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ABSTRACT 

A  review  of  the  Odyssey  lib  autonomous  underwater  vehicle  shows  that  energy  costs  asso¬ 
ciated  with  vehicle  controls  can  be  reduced  and  operational  flexibility  improved  with  rela¬ 
tively  simple,  low  cost  improvements.  Because  the  operating  speed  that  minimizes 
forward  drag  is  not  necessarily  the  same  as  that  required  for  optimum  sensor  performance, 
a  variable  speed  capability  extending  to  the  bottom  of  the  vehicle  speed  range  is  sought. 
Optimizing  Odyssey  IIB  AUV  performance  for  slower  speed  operations  and  extended 
duration  missions  necessitates  a  multi-disciplinary  review  including  control  system 
design,  hydrodynamic  performance  and  sensor  selection  and  utilization.  Reducing  the 
vehicle  controls-fixed  directional  instability  by  adding  vertical  fixed  fins,  implementing  an 
actuation  filter,  and  designing  a  model  based  adaptive  sliding  controller  improves  the  vari¬ 
able  speed  performance  and  reduces  the  control  actuation  necessary  to  provide  the  desired 
performance  level  with  energy  savings. 
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Chapter  1 


Thesis  Description 

A  review  of  the  Odyssey  13b  autonomous  underwater  vehicle  shows  that  energy  costs 
associated  with  vehicle  controls  can  be  reduced  and  operational  flexibility  improved  with 
relatively  simple,  low  cost  improvements.  Because  the  operating  speed  that  minimizes  for¬ 
ward  drag  is  not  necessarily  the  same  as  that  required  for  optimum  sensor  perfor¬ 
mance  [2]  [19]  [20],  a  variable  speed  capability  extending  to  the  bottom  of  the  vehicle  speed 
range  is  sought.  Optimizing  Odyssey  IIB  AUV  performance  for  slower  speed  operations 
and  extended  duration  missions  necessitates  a  multi-disciplinary  review  including  control 
system  design,  hydrodynamic  performance  and  sensor  selection  and  utilization.  Reducing 
the  vehicle  controls-fixed  directional  instability  by  adding  vertical  fixed  fins,  implement¬ 
ing  an  actuation  filter,  and  designing  a  model  based  adaptive  sliding  controller  improves 
the  variable  speed  performance  and  reduces  the  control  actuation  necessary  to  provide  the 
desired  performance  level  with  energy  savings. 

1.1  Motivation 

A  four  dimensional  ocean  sampling  project  called  the  Autonomous  Oceanographic 
Sampling  Network  (AOSN)  is  underway  with  MIT  Sea  Grant  College  Program,  Woods 
Hole  Oceanographic  Institution  and  North  Carolina  State  University  as  joint  participants. 
The  goal  is  an  economically  feasible  system  for  repeated  synoptic  characterization  of 
large  scale  oceanographic  features  [5].  A  network  consists  of  nodes  with  a  varying  number 
of  base  buoys  and  small  autonomous  underwater  vehicles  (AUVs).  The  AUVs  sample 
paths  in  the  network  at  desired  intervals.  Moored  base  buoys  provide  power  and  communi- 
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cation  nodes  to  complete  the  long  term  multiple  vehicle  sampling  network.  Sampled 
parameter  gradient  error  bounds  determine  vehicle  population,  tracklines,  instruments, 
node  distribution  and  sampling  frequency.  With  the  network  in  place  multiple  platforms 
for  direct  measurement  exist  within  the  defined  ocean  volume.  This  network’s  advantages 
are  synoptic  volume  coverage,  adaptive  sampling,  flexible  control,  energy  management 
and  robustness  to  component  failure. 

The  Odyssey  lib  vehicle,  developed  by  the  MIT  Sea  Grant  Underwater  Vehicles 
Laboratory,  is  planned  for  use  in  the  AOSN.  This  AUV  is  a  2.15m  long,  0.59m  diameter 
cylindrical  vehicle  with  a  single  thruster  for  propulsion  and  cruciform  shaped  control  sur¬ 
faces  mounted  in  the  stem  section.  The  four  control  surfaces  are  vertically  and  horizon¬ 
tally  mounted  and  are  actuated  in  pairs. 

The  planned  configuration  for  the  first  system  deployment  in  the  Labrador  Sea  is  for 
two  moored  buoys  to  provide  multiple  services  to  two  deployed  vehicles.  These  services 
include  an  AUV  docking  station,  navigation  beacon,  power  source  for  recharging  AUV 
batteries,  and  a  data  storage/transfer  capability.  Mooring  buoys  will  be  located  within  a 
Long  Baseline  Navigation  network  encompassing  the  area  of  interest.  The  AUVs  will 
periodically  undock,  run  a  programed  mission  profile,  collect  oceanographic  data  for 
about  four  hours  and  then  return  to  the  dock.  The  AUV  navigates  to  within  a  kilometer  of 
a  mooring  buoy  and  then  uses  an  Ultra  Short  Base  Line  navigation  system  to  home  to  the 
dock.  After  docking,  the  AUV  off-loads  sampled  data,  recharges  its  batteries  and  then 
sleeps  until  the  next  scheduled  sampling  period.  Figure  1.1  through  Figure  1.3  illustrate 
the  vehicle  configuration  and  the  docking  sequence.  In  the  first  deployment,  this  routine  is 
anticipated  to  continue  for  four  months  in  the  Labrador  Sea  through  the  winter  and  early 
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spring  months.  The  vehicles  will  aid  parameterizing  the  thermocline  breakdown,  convec¬ 
tive  plume  formation,  and  their  interaction  with  the  meso-scale  eddies  in  which  they  are 
embedded[13]. 


Figure  1.1  Odyssey  Homing  to  the  Dock 


One  vehicle  specific  problem  encountered  in  Odyssey  operations  is  the  need  for 
vehicle  controller  retuning.  Five  Odyssey  lib  vehicles  built  by  the  MIT  Sea  Grant  Lab, 
employed  for  various  projects,  require  a  different  instrumentation  set  for  most  tasks.  Each 
mission,  with  new  instrumentation,  different  mounting  configurations,  different  operating 
speeds,  etc.,  requires  controller  retuning  for  optimal  performance.  Control  gains  are 
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unique  to  each  vehicle  and  can  not  be  applied  to  vehicles  uniformly  [1].  These  complica¬ 
tions  limit  operating  speed  choices  to  the  one  selected  for  optimum  survey  results. 

Several  vehicle  behaviors  can  benefit  from  the  ability  to  increase  or  decrease  speed. 
During  docking,  slowing  to  reduce  impact  velocity  will  extend  component  lifetimes  and 
increase  the  final  homing  maneuver  effectiveness.  Obstacle  avoidance  behaviors  can 
increase  speed  to  improve  vehicle  maneuverability.  Bottom  following  algorithms  can 
incorporate  speed  increases/decreases  to  compensate  for  changing  needs  for  control 
authority  [3]. 


Figure  1.2  Odyssey  Latched  on  the  Dock 
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One  concern  for  successful  long  term  vehicle  deployment  for  the  AOSN  project  as 
well  as  long  term  AUV  success  is  increased  mission  duration [16]  [29].  The  present  Silver- 
Zinc  batteries  in  the  Odyssey  have  insufficient  battery  capacity  for  the  expected  mission 
duration  and  required  reserve  capacity.  For  the  Labrador  Sea  deployment,  a  larger,  higher 
capacity  Silver-Zinc  battery  will  be  used  [18].  With  limited  payload  capacity,  increasing 
battery  size  is  a  limited  option  for  solving  the  mission  duration  problem. 


y ik 

Figure  1.3  Odyssey  Docked 
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1.2  Research  Objectives 

This  thesis  project  has  multiple  goals  of  making  vehicle  improvements  that  enable 
changing  scientific  equipment  configurations  with  minimal  controller  maintenance  and 
allow  vehicle  speed  flexibility  during  a  mission  to  minimize  power  consumption  while 
meeting  sampling  requirements.  With  the  limitation  that  improvements  will  be  relatively 
low  cost  and  easy  to  implement,  the  concerns  addressed  by  battery[17]  and  fuel 
cell[15][24]  research  are  left  alone.  Long  term,  large  scale  changes  that  improve  drag 
characteristics  [12]  [16]  are  not  in  the  scope  of  possible  solutions  for  the  same  reason.  What 
remains  is  the  energy  expended  positioning  the  control  surfaces.  Although  actuator  energy 
requirements  are  a  fraction  of  the  propulsion  load,  for  a  small  vehicle  such  as  Odyssey 
they  are  significant  and  more  efficient  actuator  use  is  beneficial.  Any  new  controller  design 
effort  must  provide  robust  performance  for  uncertain,  changing  hydrodynamic  characteris¬ 
tics  and  varying  operating  speeds  without  controller  retuning.  Because  it  is  doubtful  that 
controller  design  alone  will  meet  the  comprehensive  goals,  the  review  includes  hydrody¬ 
namic  characteristics  and  instrumentation.  Small  improvements  in  several  areas  are 
expected  to  reinforce  each  other  and  the  cumulative  effect  enable  robust  operations  at 
varying  speeds  with  energy  savings. 

With  a  large  number  of  torpedo  or  tear  drop  shaped  AUVs,  the  results  of  this  effort 
apply  to  a  large  portion  of  the  present  AUV  population.  Some  other  AUV s  sharing  this 
basic  shape  include  the  AUSS  (Naval  Command,  Control  &  Ocean  Surveillance  Center, 
San  Diego,  CA)[25],  ORCA  (Naval  Research  Lab,  Stennis  Space  Center,  MS)[4],  Ocean 
Explorer  (Florida  Atlantic  University)  [22],  REMUS  (Woods  Hole  Oceanographic  Institu- 
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tion)[26],  the  XP-21  vehicle  (Applied  Remote  Technology),  the  UUV  vehicles  (Draper 
Laboratory)  and  the  LDUUV  (Naval  Underwater  Weapons  Center,  Newport  RI)[28]. 

1.3  Thesis  Outline 

Chapter  2  discusses  the  standard  submarine  equations  of  motion  and  the  model 
development  used  in  controller  design.  Model  simplifications  from  the  full  six  degree  of 
freedom  standard  submarine  equations  are  presented. 

Chapter  3  discusses  the  sliding  controller  design  and  the  basics  of  that  design 
approach  for  pitch/depth  and  yaw.  After  the  sliding  control  design  is  developed,  the  adap¬ 
tation  addition  is  presented. 

Additional  implementation  features  and  simulated  vehicle  performance  with  the 
sliding  controller  is  offered  in  Chapter  4.  Each  feature  is  analyzed,  isolating  its  effects,  to 
assess  its  contribution. 

Chapter  5  evaluates  directional  stability  and  reviews  performance  results  after  add¬ 
ing  fixed  vertical  fins  on  the  afterbody.  It  discusses  the  benefits  obtained  in  the  vertical  and 
horizontal  plane. 

Conclusions  and  recommendations  for  modification  implementation  are  summarized 
in  Chapter  6. 
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Chapter  2 


AUV  Dynamics 

2.1  Equations  of  Motion-Standard  Submarine  Equations 

This  chapter  examines  vehicle  dynamics  and  adapts  the  Odyssey  model  developed 
by  Hover  [10].  First,  the  full  six  degree  of  freedom  model  is  examined  and  then  simplified. 
Equations  are  then  uncoupled  for  independent  models  for  pitch  and  yaw. 


2.1.1  Coordinate  System 


The  coordinate  system  is  the  standard  submersible  coordinate  system  and  varies 
from  that  used  in  [6].  In  this  inverted  coordinate  system  z-position  increases  with  increas¬ 
ing  depth.  The  right  hand  rule  applies  to  this  coordinate  system  as  Figure  2.1  illustrates. 


Figure  2.1  Vehicle  Coordinate  System 

The  three  Euler  angles,  describe  the  coordinate  transformation  from  an  earth-fixed  to 
body-fixed  coordinate  system.  The  yaw  angle(Psi)  is  the  angle  about  the  z-axis,  with  posi¬ 
tive  angles  the  same  as  that  read  off  a  compass.  Pitch  (theta)  is  the  second  angle  of  rotation 
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and  is  measured  about  the  transverse  axis,  y,  so  that  positive  pitch  angles  are  attained  with 
the  vehicle  pitched  nose  up.  The  final  rotation,  roll  (Phi),  is  about  the  longitudinal  axis 
with  a  roll  to  starboard  being  positive.  These  Euler  angles,  taken  in  the  prescribed  order 
uniquely  describe  all  vehicle  attitudes  except  for  +/-  90  degree  pitch  angles.  Odyssey  can 
not  achieve  this  condition  so  it  is  ignored. 

Looking  first  at  the  forces  acting  on  the  vehicle,  the  translation  and  moment  equa¬ 
tions  take  the  form  shown  in  equations  (2.1)  and  (2.2).  This  includes  the  vehicle 
body,^o,t,Am ,  weight, Pw ,  buoyancy, Pb  ,  propulsion, and  control  surface  effects,  B/Z/. 

-Pi  =  pD,L,Am  +  Pw  +  pB  +  Pt  +  P>f  +  tf  (2.1) 

-$i  =  Md,l,am  +  Mw  +  +  ft4t  +  rf  x(Df  +  if)  (22) 

With  the  vector  rcm  representing  the  center  of  mass  position  measured  from  the  ori¬ 
gin  of  the  body-reference  system,  the  acceleration  vector  at  the  mass  center  in  body-refer¬ 
enced  coordinates  is: 


Clem  —  V  +  VVXV+1VX  r  cm  +  WX  ( W  X  T  cm)  ^2  ^ 

The  vector  r  =  [*  ■>,  z]  is  the  vehicle  position  in  the  earth  coordinate  system.  Eq  (2.4) 
represents  the  vehicle’s  body  referenced  velocities.  The  vehicle  angular  rotation  vector 
through  the  Euler  angles  is  represented  by  c)  shown  in  Eq.  (2.5). 


(2.4) 


&  -  [p  qr\ 


(2.5) 
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When  working  with  a  symmetrical  body,  it  is  convenient  to  have  the  origin  be  the 
longitudinal,  transverse  and  normal  body  axes  intersection.  In  this  situation,  if  the  cross 
moments  of  inertia  are  negligibly  small  then  the  rigid  body  equations  of  motion  are 
expressed  as: 


1  0  0  0  Zcm  —ycm 

2  2 

pqycm-q  Xcm-r  xcm  +  przcm  +  qw-  rv 

't 

fi=  -m 

0  10  —  Zcm  0  Xcm 

V 

_! 

-  m 

2  2 
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2.1.2  Coordinate  Transformation 

The  transformation  matrix  converting  the  vehicles  trajectory  from  an  earth-fixed  ref¬ 
erence  frame  to  a  body-fixed  reference  frame  is  given  in  Eq.  (2.9). 


Xbody  — 


cos9cos\|/ 

sin  <|>  sin  0  cos  \\f  -  cos  <)>  sin  \|/ 
cos  0  sin  0  cos  \|/  4-  sin  <|)  sin  \\f 


cos0sin\|/ 

sin<()sin0sin\|/  +  cos  <|>  cosy 
cost)) sin 0 sin y  -  sin cos \\f 


~sin0 
sin<()cos0 
COS  (|)  cos  0 


Xinertial 


(2.9) 
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Where  C  is  the  matrix  defined  in  Eq.  (2.9), 


Xbody  —  C  *  Xinertial 


(2.10) 


The  corresponding  transformation  matrix  for  the  Euler  angle  rates  in  transformation 
matrix  terms  is  similar. 


E 


1  sin<|>tan0 
0  cos(|) 

0  sin  (j)/ cos  0 


cos  <|>  tan  0 
-sintj) 
cos<|>/ COS0 


=  T(P)ti 


(2.11) 


2.1.3  Weight  and  Buoyancy 

Weight  and  Buoyancy  forces  in  earth  coordinates  are  written  here. 


Pw 


(2.12) 


Pb 


0 

0 

-B 


(2.13) 


Moments  are  obtained  with  the  vector  cross  products  of  tw  and  >b  . 


Mw 


ywFwz—ZwFwy 
ZwFwx-xwFwz 
xwFwy  —  ywFwx 


(2.14) 


-yBFBz  +  ZBF  By 
-ZBFBx  +  XBFBz 
-XbF By  +  yBFBx 


(2.15) 
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2.2  Linearized  Dynamics 

At  this  point  the  rigid  body  dynamics  and  the  most  straightforward  forces  have  been 
considered.  For  the  remaining  forces  a  theoretical  and  experimental  method  combination 
is  used.  Hover  uses  a  variety  of  methods  [10]to  model  fluid  forces.  Hoemer’s  curves 
[8][9]are  used  to  develop  lift  and  drag  expressions  for  the  body.  Newman’s  strip  theory 
[14]  is  used  for  lateral  drag  and  body  and  fin  added  mass  calculations.  Momentum  theory 
[27]  is  used  for  propulsion  forces.  Common  models  for  fin  forces  are  taken  from  [7]. 

Dynamics  are  linearized  because  linear  calculations  give  insight  into  basic  dynamic 
response.  This  method  shows  directional  stability  and  non-minimum  phase  behavior. 
Hover  also  opined  [10]  that  when  in  straight  and  level  flight  the  multi-input,  multi-output 
linear  control  methodology  works  well.  Model  linearization  about  an  operating  speed  is 
not  limiting  in  practice.  Operationally,  speed  is  a  fairly  constant  parameter  and  changes, 
typically  made  in  a  step-wise  manner,  are  relatively  infrequent.  Gain  scheduling  using  an 
array  of  operating  speeds  produces  a  gain  structure  that  gives  the  desired  performance. 
This  unpopular  scheme  is  complex  and  requires  repetitive  design  effort  at  each  operating 
point.  The  guaranteed  tracking  performance  with  modeling  errors  and  parameter  uncer¬ 
tainty  makes  the  sliding  controller  an  attractive  option.  With  drag  coefficients  being  Rey¬ 
nolds  number  functions,  each  new  operating  speed  introduces  new  parameter 
uncertainties.  Adding  the  adaptation  scheme  to  the  sliding  controller  furthers  the  capabil¬ 
ity  to  provide  guaranteed  performance  as  the  parameter  uncertainty  due  to  changing 
speeds  or  modeling  errors  is  transparent  to  the  controller. 

The  dynamics  are  linearized  with  yaw  and  pitch  coupled  because  axial  torque  from 
the  thruster  leads  to  a  static  roll  angle  which  cannot  be  corrected  when  elevator  and  rudder 
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surfaces  are  ganged.  Cross  coupling  between  elevator  motion  and  heading  and  rudder 
motion  and  pitch  is  assumed  to  be  small.  In  practice,  a  primary  vehicle  ballasting  consid¬ 
eration  is  minimizing  cross  coupling.  The  decoupled  equations  are  found  by  setting  the 
static  roll  angle  <j>  to  zero.  Also  the  variables  are  linearized  about  an  equilibrium  point  by 
using  an  overbar  (0 )  to  represent  equilibrium  and  a  tilde  (0 )  to  represent  deviations  from 
equilibrium.  Assumptions  made  for  linearizing  are: 

1.  |v|  =  |p|  =  « 

2.  Pitch  0  and  yaw  \j>  deviations  are  small. 

3.  Second  and  higher  order  terms  are  negligibly  small. 

With  these  assumptions  the  Inertial  Forces  and  Moments  linearization  in  steady  flight 
becomes: 


1  0  0  0  Zcm  —ycm 

0 

Pi-  -m 

0  10  — Zcm  0  Xcm 

V 

-m 

ru 

A 

0  0  1  ycm  —Xcm  0 

w 

-qu 

The  rotational  terms  are: 


(2.16) 


0 

—Zcm 

ycm 

0 

-qu 

-ru 

Iw-m 

Zcm 

0 

—Xcm 

V 

-m 

qu 

0 

0 

-ycm 

Xcm 

0 

ru 

0 

0 

(2.17) 


Yaw  and  Pitch  angles  are  assumed  to  be  small,  while  the  static  roll  angle  may  be  ten 
degrees  or  more,  usually  due  to  induced  torque  from  the  thruster.  For  small  pitch  and  yaw 
perturbations,  the  coordinate  transformation  matrix  can  be  written  as  the  sum  of  C  and  C . 
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(2.18) 


1  0  0 


C  = 


0  cos<|>  sin  6 
0  —  sin  ft)  cos  6 


0 


sin<j>0  —  cos<|)\(/ 
cos<j)0  +  sin<j>\|/ 


V 

0 

0 


-0 

0 

0 


(2.19) 


2.3  Weight  and  Buoyancy 

Using  the  transformation  matrix  decomposed  form,  weight  and  buoyancy  terms  are 
expressed  as 


Fw  =  (C+C) 


0 

0 

-we 

0 

= 

0 

+ 

0 

w 

w_ 

0 

(2.20) 


0 

0 

BB 

Fb  =  (C+C) 

0 

= 

0 

+ 

0 

-B 

-B 

0 

(2.21) 


Fw  +  Fb  = 


0 

(B-w)  e 

0 

+ 

0 

W-B 

0 

(2.22) 


ywPwz-ZwPwy 

0 

ZwFwx-XwFwz 

= 

+ 

-ZgW0 

Xwftwy  —  ywP  Wx 

0 

yoW0 

(2.23) 
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(2.24) 
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-yBpBz  +  ZbP  By 

-yBB 

0 

-ZbFbx  +  XbFbz 

= 

xbB 

+ 

zb  Be 

—XbP  By  +  yBpBx 

0 

-yBBB 

r 

n 

r 

i 

Mw  +  A4b  = 


ywW-yBB 

0 

xbB-XwW 

+ 

(ZbB-ZwW)B 

0 

(ywW-yBB)B 

(2.25) 


2.4  Body  and  Fin  Fluid  Forces  and  Moments 

The  linearized  expressions  taken  from  [10]  for  the  fluid  forces  and  moments  acting 
on  the  vehicle  body  are: 


Pd 


,  L,  Am 


2Xu\u\UU  4-  XuU 
YuvUv  +  YvV  4-  Yfr 
ZuwUW  4-  ZwW  +  ZqQ_ 


(2.26) 


7&D 


,  L,  Am 


Kp 

2Mu\u\UU  +  MuwUW  +  MwW  +  A4q(j 
2Nu\u\uu  +  Nuvuv  +  Afvv  +  Nfr 


(2.27) 


In  a  linear  approximation  during  steady  flight,  fin  forces  consist  only  of  lift  and  are  written 
as  follows  using  the  subscript  notation  Xab .  The  a  is  either  r  for  rudder  or  e  for  elevator. 
The  b  is  a  number  one  or  two  to  differentiate  the  two  fins. 

•  Zei  Ze 2  Y r i  Yri  Forces  from  the  two  fins 

•  p  fluid  density 

•  Ae i  Ae 2  Ari  Ar2  Fin  Surface  Areas 

•  Cle\  del  Clri  dri  Lift  coefficients 

•  8*i  Se2  8ri  8r2  Fin  Position  in  radians 

Ze  1  =  -0.5p AelClel(u\V  +  uy el P  -  UXeiq  +  U28el)  (2.28) 
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(2,29) 


Zel  =  -0.5pAe2Cle2(u\V  +  Uyelp 


-  UXe2q  +  uhe2) 


Yrl  =  0.5  pArl  C7rl  (—  UV  —  UWrW  +  M£rl  /?)  +  M  5rl 


(2.30) 


-  -  -  -2 

y>2  =  0.5pAr2C/r2(-MV-MWr2r  +  MZr2/>)  +  M  5r2  (2.31) 

The  moments  for  the  rudder  and  elevator  are  applied  on  the  appropriate  moment  arms. 

2.5  Linearized  Model  for  Control  System  Design 

Hover  considers  only  a  neutrally  buoyant  vehicle  in  his  model  development  so 
Weight  and  Buoyancy  terms  are  absent  in  his  model.  The  usual  practice  is  to  have  the 
vehicle  ballasted  slightly  positive  for  easier  recovery.  This  practice  significantly  affects 
vehicle  dynamics  when  slower  operating  speeds  are  attempted.  With  positive  buoyancy 
considered,  the  linearized  model  becomes: 


lx  =  A%  +  Bit 


(2.32) 


*  -  [uvwpqrxyz 


4>  0  v] 


(2.33) 


^  -  5r  5c  l] 


(2.34) 
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(2.35) 


o 

o 

o 

o 

o 

o 

o 

b 

© 

© 

© 

© 

£ 

CQ 

CQ 

o 

o 

1 

o 

£ 

y 

o 

© 

b 

© 

© 

© 

cq 

cq 

1 

05 

£ 

is? 

o 

o 

o 

o 

O 

o 

o 

© 

© 

© 

© 

© 

o 

o 

o 

o 

o 

o 

© 

© 

© 

© 

© 

© 

o 

o 

o 

o 

o 

o 

© 

© 

© 

© 

© 

© 

o 

o 

o 

o 

o 

o 

© 

© 

© 

© 

© 

© 

I 

c 

o' 

I 

b 

s 

I 

b 

i*. 

a 


o 

£ 


b 

c 

U 

I 

o 

s 

E 

i 

b 

S<. 

a 


o  o  o 


o 

o 


o  o 


b* 


2 

c 

u 

+ 

b 

E 

+ 

b 

a 

N 

I 


o  o  o  © 


s 

i 

b 

£ 

a 

N 


o  o 


2 

e 

u 


e> 

£ 

E 

I 

b 

a 

§ 

o 

c 

*k 

£ 

R 

§ 

+ 

b 

£ 

a 

I 


-©-  -©- 


o  o  o  o  o 


o  o  o  o 


o  o 


o  o  o 


O' 

I 

b 

5 


o 

a 

a  o  o 

X 

<N 


.r 

u 

I 

b 

a 


-e-  -e- 


o  o  o  o  o 


23 


(2.36) 
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(2.37) 


B  = 


— 0.5  pAfinCdfin2U  —  0.5  P  Afin  Cdfin  2  U  0 


CfinU 

0 

0 

0 

2 

XfinCfinU 

0 

0 

0 

0 

0 

0 


0 

-CfinU1 

0 

2 

Xfin  Cfin  U 
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0 

0 
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0 

0 

0 


0 

W-B 

ywW-yiiB 

XbB-XwW 

0 

0 

0 

0 

0 

0 

0 


The  buoyancy  and  weight  terms  are  split  between  the  A  and  B  matrices  for  conve¬ 
nience.  The  vertical  moment  arm  is  included  in  the  A  matrix  and  is  one  of  the  terms 
adapted  for  control  purposes.  The  other  buoyancy  and  weight  term  is  more  accurately 
determined  prior  to  vehicle  deployment.  It  is  considered  a  constant  pitch  input  and  is 
included  in  the  B  matrix. 


2.6  Models  for  Pitch  and  Yaw 

Separate  uncoupled  models  for  pitch  and  yaw  are  presented  here.  With  the  vertical 
and  horizontal  control  surfaces  ganged,  pitch  is  uncontrollable.  This  makes  uncoupled 
models  an  attractive  model  simplification.  Maintaining  a  separation  between  the  center  of 
gravity  and  buoyancy  effectively  limits  the  roll  angles  to  less  than  ten  degrees.  The  vehicle 
states  chosen  are  those  necessary  to  show  the  vehicle  instability  in  the  yaw  plane  in  the  lin¬ 
earized  model.  The  corresponding  pitch  plane  parameters  are  used  for  a  similar  pitch  plane 
model. 
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2.6.1  Model  for  Pitch 


=  A0X  +  J90M 

(2.38) 

-  [w  q  0] 

(2.39) 

-  [5e  ^ 

(2.40) 

/e 


m  +  Maw  —  mXG  -  XmaMaw  0 

-  mXG  -  XmaMaw  Jyy  +  Jamy  0 

0  0  1 


(2.41) 


Ae 


ZuwU  -  CfinU  -  ZuqU  +  mU  +  CfinUXfin  B-W 
-  MuwU  +  Cfin  UXfin  Muq  U  -  m  Uxg  -  Cfin  UXfin 2  zbB  -  zgW 
0  cos  4»  0 


(2.42) 


-CfinU2  W-B 

XfinCfinU2  XbB-XwW 
0  0 


(2.43) 


2.6.2  Model  for  Yaw 


I\\fX  =  A\ \fX  +  Byii 


(2.44) 


i  = 


(2.45) 


u  =  8r 


(2.46) 


26 


h 


m  +  Mav  mXG  +  XmaMav  0 
mXG  +  XmaMav  Jzz  +  Jamz  0 
0  0  1 


(2.47) 


YuvU—CfinV  YurU  —  mU  —  CfinUXfin  0 
Av  =  NuvU  -  CfinUXfin  NurU  -mUXG  -  CfinUXfin  0 
0  cos(|)  0 

CfinU2 

2 

XfinCfin  U 

0 


(2.48) 


2.7  Summary 

The  models  for  pitch  and  yaw  are  the  linearized  models  used  by  Hover  for  the  PHD 
controller  design  with  weight  and  buoyancy  added.  The  hydrodynamic  parameter  esti¬ 
mates  using  Hoemer’s  curves,  strip  theory  and  the  like,  were  refined  in  the  tow  tank  at 
Draper  Lab  using  a  1.45  m/sec  forward  speed[ll].  These  values  are  used  later  to  show  the 
yaw  plane  instability.  With  two  uncoupled  models  for  pitch  and  yaw  that  take  into  account 
net  positive  buoyancy  and  vehicle  dynamics  in  sufficient  detail,  the  controller  design  task 
presents  itself. 
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Chapter  3 


Control  System  Design 

Sliding  mode  control  is  a  nonlinear  tracking  control  system  method.  Given  a  nonlin¬ 
ear  model  with  bounded  parameter  uncertainty  and  bounded  disturbances,  the  method 
yields  a  nonlinear  feedback  controller  with  guaranteed  tracking  and  stability  properties. 
The  system  is  designed  to  follow  a  commanded  state  trajectory  and  drive  the  tracking  error 
to  zero.  The  tracking  error  dynamics  are  actually  specified  and  used  as  a  constraint  on  the 
system.  The  object  is,  given  the  given  tracking  error  dynamics,  define  a  control  law  such 
that  those  dynamics  are  achieved  by  the  closed  loop  system  with  modeling  errors  and  dis¬ 
turbances.  The  sliding  controller  development  presented  here  is  adapted  from  Slotine  and 
Li  [21]. 

Sliding  control  is  formulated  in  this  chapter  for  heading  and  depth/pitch  control. 
Speed  control  is  not  addressed  as  the  present  instrumentation  is  inadequate  for  a  constant 
speed  controller.  The  vehicle  uses  open  loop  speed  control  because  speed  is  not  measured 
in  any  axis. 

3.1  Robust  Sliding  Control  Overview 

Consider  a  single  input  dynamic  system 

x  =  f(x )  +  b(x)u 

where  x  is  the  scalar  of  interest,  the  scalar  u  is  the  control  input  and  the  state  vector 
xd  =  T  •  "  x»-  d1  T results.  The  terms  /  and  b  are  generally  nonlinear  state  vector 

Xd  Xd  Xd  . . .  Xd 
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functions  and  need  not  be  exactly  known.  What  is  known  about  f  and  b  is  that  they  are 
upper  bounded  by  a  known  continuous  function  of  x .  Additionally  b  is  of  known  sign.  The 
control  problem  is  to  get  the  state  vector  *  to  track  a  specific  time  varying  state  xd  in  the 
presence  of  model  imprecision  on  f(x)  and  b(x) . 

Looking  at  notation,  let  x=  x-xd  be  the  tracking  error  in  the  variable  *  and 
x  =  [i  x  x  ...  be  the  tracking  error  vector.  Defining  a  time  varying  surface  S(t)  in 

the  state  space  Rn  by  the  scalar  equation  s{x\t)  =  0  where  the  desired  error  dynamics  are 
defined  to  be  of  the  form 


s{x\t)  = 


(3.1) 


The  term  X  is  a  strictly  positive  constant  and  is  considered  the  control  bandwidth  as 
an  analogy  to  linear  control  theory.  It  is  the  weighting  factor  in  the  weighted  sum  of  the 
position  and  velocity  error.  If  n  =  2 ,  then  s  =  x  +  Xx.  This  corresponds  to  a  line  that  moves 
with  xa  =  \^xd  T  and  has  a  -X  slope.  The  system  state  does  not  follow  xa  =  \^xd 
exactly  but  remains  on  the  line  s(x;t)  =  0.  The  desired  closed  loop  dynamics 
s  =  x  +  Xx  =  0  are  exactly  achieved  and  accordingly  x(t)  =  x(0)e-x' .  The  simplified  first 
order  problem  of  keeping  s  =  0  can  now  be  achieved  by  choosing  the  control  law  u  such 
that  outside  S(t ) 


ifs-nH  (3.2) 

where  p  is  a  strictly  positive  constant.  This  states  that  the  squared  distance  to  the  surface 
as  measured  by  s2  decreases  along  all  system  trajectories.  Once  on  the  surface,  the  system 
trajectories  remain  on  the  surface.  Some  disturbances  or  uncertainties  can  be  tolerated, 
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while  keeping  the  system  trajectories  on  the  surface.  Eq.  (3.2)  is  derived  in  [23]  from 
Lyapunov  theory  and  the  discontinuous  differential  equation  dynamics. 

Thinking  about  the  SISO  system,  the  control  gain  b(x)  is  not  precisely  known  but  is 
bounded,  so  that  0 <  bmm  <b<  bmax .  Because  the  control  input  enters  multiplicatively  in  the 
dynamics,  our  estimate  b  of  gain  b  is  the  geometric  mean  of  the  bounds; 

b  —  ( bminbmax )  (3.3) 

Bounds  can  be  written  in  the  form, 

p_1<f<p  (3.4) 

b 


where 


P  -  ( bmax/bmin )  (3*5) 

The  term  p  is  called  the  design  gain  margin  as  an  analogy  to  linear  control  design. 

Considering  a  second  order  system,  x  =  f  +  bu.  In  order  to  have  the  system  track 
x  =  xd  the  sliding  surface  is  defined  as  5  =  x  +  Xx.  Then,  we  can  say  that 

s  =  x  —  xd  +  Xx  =  /  +  bu  -  Xd  +  Xx .  (3.6) 

The  best  continuous  control  law  approximation  that  achieves  s  =  0  is  u  =  -f  +  xd-Xx. 
Adding  a  feedback  term  to  satisfy  sliding  conditions  because  of  uncertainties  in  /,  the  con¬ 
trol  law  becomes  u  =  b~1[u-ksgn(s)] . 

Choosing  the  control  law  as  stated  leads  to  a  discontinuous  control  action  called 
dithering  or  chatter  when  the  s  =  0  state  is  reached.  To  avoid  exciting  unmodeled  dynam- 
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ics  a  continuous  control  law  is  developed  by  establishing  a  boundary  layer  surrounding  the 
switching  surface.  The  layer  is  defined  as 


B{t)  =  {x,  |s(*;f)l  <<£}  (3.7) 

where  <t>  is  the  boundary  layer  thickness.  The  control  law  becomes 

u  =  b  -  ksat(s/<&)]  (3.8) 

The  control  discontinuity  smoothing  inside  B(t)  essentially  assigns  a  low  pass  filter  struc¬ 
ture  to  the  5  local  dynamics,  thus  eliminating  chatter. 

3.2  Controller  Design 

In  designing  the  model  based  adaptive  sliding  controller,  first  the  robust  sliding  con¬ 
troller  development  is  presented  and  then  subsequently  the  adaptation  method.  The  control 
model  is  further  simplified  by  taking  only  the  moment  equations  for  both  pitch  and  yaw. 
This  is  very  straightforward  for  heading  control.  For  depth  and  pitch  control  an  early  PID 
controller  uses  a  transfer  function  to  generate  a  desired  pitch  based  on  a  depth  error.  The 
present  PID  controller  uses  an  inner/outer  loop  control  method  for  pitch  and  depth.  This 
same  approach  is  used  for  the  sliding  controller  with  some  modifications. 

3.2.1  Robust  Sliding  Controller  Design  for  Pitch 

Taking  the  moment  equation  for  pitch,  we  can  derive  the  robust  sliding  controller  as 
follows: 
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(-  mXG - XmaMaw)w  +  (Jyy  +  Jamy)q  =  (- MuwU  +  CfinUXfin)w  +  (MuqU -mUxG -  CfinUXfin2)q 

+  ( zbB  -  zgW)Q  +  (XfinCfinU2)5e  +  xbB-XwW  (3.10) 

Arranging  the  state  variables  in  a  linear  combination 


u  =  U  (3.11) 

a  1  =  Jyy  +  Jamy  (3.12) 

a'Z  —  —Muq  +  mXG  +  CfinX  fin2  (3.13) 

a3  =  -mXG-XmaMaw  (3.14) 

aA  =  Muw-C  finXfin  (3.15) 

a5  =  zbB-zgW  (3.16) 

a6  =  ( XwW-xbB )  (3.17) 

2 

aid  +  a2uQ  +  a3w  +  aAuw  +  a5sin0  +  <z6cos0  =  -XfinCfinu  be  =  Ae  (3.18) 
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Defining  the  following  terms: 


0  =  0-0rf 


0  =  0-0d 


0  =  0-04 


Here  a  reference  velocity,  Or ,  is  formed  by  shifting  the  desired  velocity,  Od ,  according  to 


the  position  error,  0 . 


0  r  —  Od  —  'Xq  0 


=  0  +  ^0  =  0  — 9r 


0r  —  Od  —  ^  0 


jg  =  0-0r 


Define  a  Lyapunov  function  candidate,  V(t),  a  positive  definite  function: 


V  =  |a!0e)2 


Differentiating, 


V  =  ,%alse  +-.%al(.%) 


Substituting  4  =  0-0r, 
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(3.28) 


V  =  Jq  (al0- aidr)  +  aljg 
Substituting  a  1 6  from  the  system  dynamics  yields 

al0  =  Ae  -  (a2«0  +  a3w  +  aAuw  +  a5sin0  +  a6cos0)  (3.29) 

Using  the  relations 


Sf)  =  0  +  ^0  =  0-0r 

(3.30) 

0  =  .%  +  0r 

(3.31) 

then 


al0  =  Ae-[(a2w(.%  +  0r)  +  a3H,  +  a4ww  +  a5sin0  +  a6cos0)]  (3.32) 

V  =  (Ae -  [a2w(.%  +  0r)  +  a3w  +  aAuw  +  a5sin0  +  o6cos0]  -  al0r)  +  «!(•%  )  (3.33) 


V  =  jg  (Ae -al0r -a2uQr-a3w  -  aAuw  -  a5sin0  -  a6cos0)  (3.34) 

Define  the  control  input  to  be: 

Ae  =  Ae-kQ  sgn(s))  (3.35) 

If  the  dynamics  are  exactly  known,  Ae  is  the  control  input  that  makes  V  =0. 

Ae  =  al0r  +  a2uQr  +  a3w  +  aAuw  +  a5sin0  +  a6cos0  (3.36) 

We  then  have, 
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V  -  %(Ae-A@  sgn(x))-al0r-a2M0r-a3w-fl4«iv-fl5sin0-a6cos0) 


(3.37) 


V  =  %  (Ae-al'Qr-a2uQr-a3w-a4uw-a5sm6-a6cosQ)-kQ  sgn(i))  (3.38) 

Given  bounds  on  the  modeling  errors  al,a2,a3,a4,a5,a6  we  see  that  choosing  k 
such  that  k  >  |al 0r  +  a2uQr  +  a3w  +  a4uw  +  a5sin©  +  a6cos©|  + 1|  .With  T|  positive,  the  sliding 
condition  V  <-t)5  is  satisfied.  This  guarantees  that  the  surface  s  =  0  is  reached  in  a  finite 
time  and  that  once  on  the  surface,  the  trajectories  remain  on  the  surface  and  therefore  tend 
to  Ba  exponentially. 

3.2.2  Adaptive  Controller  Design  for  Pitch 

Given  a  desired,  bounded  trajectory  and  with  some  parameter  uncertainty,  the  adap¬ 
tive  controller  design  problem  is  to  derive  a  control  law  for  torque  and  estimation  law  for 
the  parameter  uncertainty  such  that  Bd-B .  Let  us  define  a  =  a-a  with  a  being  a  constant 
vector  of  uncertain  parameters  describing  the  vehicles  mass,  inertia  and  hydrodynamic 
properties  and  a  is  its  estimate. 

Consider  a  Lyapunov  Function  candidate  V  =  +^a7r"1a>0  where  r  is  a 

symmetric  positive  definite  matrix.  Differentiating  and  substituting  from  Eq(3.34)  from 
the  discussion  above  yields: 

~T  _1_ 

V  =  .% (Ae - al0r- a2uBr  —  a3w - a4uw- a5sin0 -a6cos0)  +  a  T  a  (3.39) 

Defining  a  vector  Y  and  a  such  that  the  terms  are  linearly  dependent  on  a . 

Define  the  known  matrix  Y  to  be: 

Y  =  [@r  uBr  w  uw  sin0  cos©]  (3.40) 
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so  that, 


a  -  [al  a2  a3  a4  a5  a6] 


(3.41) 


resulting  in 


al0r  +  a2u9r  +  a3w  +  a4uw  +  a5sinQ  +  a6cos6  =  Ya  (3.42) 

Taking  the  control  law  to  be 

Ae  =  Ya-k  %  (3.43) 

which  includes  a  “feedforward”  term  Ya  in  addition  to  a  simple  PD  term,  ^  %  ,  leads  to 

V  =  ^(YS-Ae^l  +  aV'i  (3.44) 

Updating  the  parameter  estimates  a  according  to  the  correlation  integrals 


i  =  -ty\ 


(3.45) 


then  yields 


V  =  -*e%2<  0  (3.46) 

This  implies  that  the  output  error  converges  to  the  surface  5  =  0  which,  given  the  fil¬ 
ter  like  definition  of  s  shows  that  0  and  0  tend  to  0  as  t  tends  to  infinity.  Therefore  the  sys¬ 
tem  global  stability  (boundedness  of  the  vectors  0,  0  and  a),  and  the  tracking  error 
convergence  are  guaranteed  by  the  adaptive  controller. 
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3.2.3  Limiting  adaptation 


Adding  a  dead-zone  to  shut  adaptation  off  for  small  tracking  errors  to  prevent  noise 
and  disturbances  from  causing  parameter  drift,  the  equations  are  modified  as  follows: 

%a  =  (3-47) 

V  =  +|arr-1  a>0  (3.48) 

al  >  0 

r_1  =  r_r  >  o 

a  =  a-a  (3.49) 


V  =  +  a  r  'a  (3.50) 

V  =  ,%a  (Ae  -  Ya)  +  a  r  *a  (3.51) 

Ae  =  Ya-^%  (3.52) 

V  =  SqA  (Ya-Ag  %  -  Ya)  +  aT~la  =  ,%A  (Ya-Ag  % )  +  aV^a  (3.53) 

i  =  -TY\a  =  a  (3.54) 

v  =  (arr_1  +  J0A  Y)a  -  %A  •%  (3.55) 
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V  =  -%A  ^)  •%  =  -^A  k)  ^%A  +  ^  - ^  )  -^0  ^  1%^ 


(3.56) 


Ae®0  >0 


(3.57) 


V<-*e(%A)2<° 


(3.58) 


3.3  Depth 

Looking  first  at  the  equations  for  rigid  body  dynamics  we  know  that 

z  =  wcos0-Msin0  (3.59) 

z  =  vvcos0  -  wsin00  -  wsin0  -  mcos00  (3.60) 

Grouping  terms,  we  can  write 

z  =  [videos©  -  iisin0]  +  [-wsin0-MCOS0]0  (3.61) 

z  =  f(Q)  +  b(Q)u  (3.62) 

From  these  equations,  it  is  seen  that  depth  control  and  pitch  control  are  inter-related. 
Altering  pitch  is  the  only  method  of  changing  or  maintaining  depth  with  a  single  set  of  ele¬ 
vators.  Using  the  same  methodology  the  following  terms  are  defined. 

z  =  z-zd  (3.63) 

z  =  z-zd  (3.64) 

i  =  z-zd  (3.65) 
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Here  a  reference  velocity,  ir ,  is  formed  by  shifting  the  desired  velocity,  id ,  according  to 
the  position  error,  z  . 

ir  =  id-Xzi  (3.66) 

sz  =  z  +  XzZ  =  i-ir  (3.67) 


Zr  =  Zd-XzZ 


(3.68) 


Sz  =  Z~Zr 


(3.69) 


Choosing  a  control  law 


s  =  z-zd  +  Xz  =  /  +  bu-Zd+  Xz 


(3.70) 


U  =  -f  +  Zd-Xz 


(3.71) 


u  —  b  \u-kzSz]  (3.72) 

The  depth  input,  Bd,  is  a  quantity  used  by  the  pitch  controller.  It  is  a  relatively 
straight  forward  matter  to  back  difference  and  integrate  0d  to  get  the  remaining  two  quan¬ 
tities,  Bd  and  Bd ,  used  by  the  pitch  controller.  What  remains  is  to  select  an  appropriate 
value  of  Xz  and  Xe .  With  the  end  result  of  the  pitch  sliding  controller  being  that  pitch 
behaves  as  a  linear  system,  it  is  straight  forward  to  make  the  restriction  that  Xz  <  Xq/4  . 
Because  the  outer  depth  loop  generates  the  Bd,  Bd,  and  Bd ,  maintaining  a  large  difference 
between  the  two  bandwidths,  allows  the  pitch  and  depth  loop  dynamics  to  be  evaluated 
independently  and  without  adverse  excitation  between  the  two.  The  controller  equations 
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are  presented  in  the  Appendix. 


3.4  Yaw 

Talcing  the  moment  equation  for  yaw  the  robust  sliding  controller  is  derived  as  fol¬ 
lows: 


-j 

V 

r  i 

V 

mXG  +  XmaMav  Jzz  +  Jamz  0j 

r 

_<P_ 

-  [ NuvU  -  CfinUXfin  NurU  -mUXG  -  CfinUXfin2  Oj 

r 

_<P_ 

+ 


Xfin  Cfin  U 


(mXG  +  XmaMav)v  +  (Jzz  +  Jamz)r  =  (NuvU  —  CfinUXfin)v  +  (NurU  —  mUXG  —  CfinUXfin  )r  (3.74) 

2 

+  ( XfinCfinU  )5r 

Arranging  the  state  variables  in  a  linear  combination 


u  =  U 

(3.75) 

a  1  =  Jzz  +  Jamz 

(3.76) 

a2  =  mXG  +  XmaMav 

(3.77) 

a3  =  Nur  —  mXG  —  CfinXfin 2 

(3.78) 

a  A  =  Nuv  —  CfinXfin 

(3.79) 
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2 

alV]jr  +  a2v  +  a3w\j/  +  a4MV  =  -XfinCfinu  8r  =  A r 


(3.80) 


Defining  the  following  terms: 

\fr  =  \\f  -  \\fd  (3.81) 

Here  a  reference  velocity,  \\ ir ,  is  formed  by  shifting  the  desired  velocity,  \j/<z ,  accord¬ 
ing  to  the  position  error,  \jir . 


\j/r  =  \jf<i-Xy\jf 

(3.82) 

Jy  =  \j/  +  Xy\jir  =  \j/-Vj/r 

(3.83) 

Sy  = 

(3.84) 

Adding  a  dead-zone  to  shut  adaptation  off  for  small  tracking  errors  to  prevent  noise 

and  disturbances  from  causing  parameter  drift,  the  equations  are  modified  as  follows: 

SyA  =  Sy 

(3.85) 

Define  a  Lyapunov  function  candidate,  V(t),  a  positive  definite  function: 

V  =  iflliyi  +  ^arr-1a  >  0 

(3.86) 

al  >0  r_1  =  r'r>0  a  =  a-a 

Differentiating, 

~r  i  „ 

V  =  alsy  +  a  T  a 

(3.87) 
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Defining  a  vector  Y  and  a  such  that  the  terms  are  linearly  dependent  on  a .  Define 
the  known  matrix  Y  to  be: 

Y=  [yrv«y,«v]  <3-88) 

so  that, 

a  =  [ol  a2  a3  a4^  (3.89) 

resulting  in 

tfliy  =  al(V|jf-\|/r)  =  Ae-Ya  (3.90) 

V  =  5VA  (Ac  -  Ya)  +  aV'i  (3.91) 

Taking  the  control  law  to  be 

Ac  =  Ya  -kySy  (3.92) 

ly  ^  0 

which  includes  a  “feedforward”  term  Ya  in  addition  to  a  simple  PD  term,  k^sy ,  leads  to 

V  =  sv£i(Ya-kySy-Ya)  +  aTr~1a  =  jva  (Ya-kySy)  +  aV^a  (3.93) 

Updating  the  parameter  estimates  a  according  to  the  correlation  integrals 

i  =  -TY7^  =  a  (3.94) 

V  —  (a  r  +  SyAY)a  —  $\yA  (3.95) 
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(3.96) 


V  =  —  kySy  =  —  Sy&ky  ^SyA  +  —  — Ai|/(svA)  +  ^,9\|/|5, 


*V<IV>0  (3-97) 

V<-V(5va)2<0  (3.98) 

If  u  and  w  are  bounded,  then  V  >  0  and  V  <  0  which  means  V  is  bounded.  Thus  sw,  a 
are  boundeda .  being  bounded  implies  a  is  bounded. sv  .  being  bounded  implies  \fr,  \j>  are 
bounded.  Assuming  that  \yd,  \j id  are  bounded  then  \|/,  \j/  are  bounded.  For  positive  values 
of  u,  with  A r  bounded  then  \|>  is  bounded.  Assuming  that  xj id  is  bounded,  then  i  is 
bounded  and  V  is  bounded  so  V  0  by  Barbalat’s  Lemma.  So  svA  0 . 

Equations  used  in  the  controller  are  in  the  Appendix. 

3.5  Summary 

Model  based  adaptive  sliding  controllers  for  depth/pitch  and  yaw  are  designed  using 
the  uncoupled  models  developed  in  Chapter  2.  Model  parameter  adaptation  is  used  to 
accommodate  slowly  changing  parameters  and  to  ensure  tracking  and  stability  in  the  pres¬ 
ence  of  modeling  errors  and  disturbances. 
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Chapter  4 


Control  Implementation  and  Simulation 

4.1  Implementation 

Chapter  4  discusses  several  controller  implementation  details.  After  state  estimation 
and  the  trajectory  generator,  the  other  implementation  details  can  be  considered  fine  tun¬ 
ing  promoting  robust  variable  speed  performance  while  minimizing  actuator  motion  and 
power  consumption.  A  model  and  trajectory  generator  that  compensate  for  slowing  and 
the  resulting  reduced  fin  and  body  lift  forestall  the  onset  of  stall  conditions.  Compensating 
for  this  reduced  lift,  allows  the  vehicle  to  achieve  a  less  aggressive  trajectory  without 
becoming  uncontrollable  in  depth  or  losing  steerageway.  The  actuation  filter,  based  on  the 
physical  gear  backlash,  dramatically  reduces  fin  motion.  Model  parameter  adaptation 
improves  tracking  performance  by  continually  refining  the  model  estimates. 

4.2  State  Estimation 

A  number  of  states  utilized  by  this  controller  are  not  measurable  with  the  current 
vehicle  instrumentation.  The  states  selected  are  the  minimum  necessary  to  capture  the  rel¬ 
evant  vehicle  dynamic  instabilities.  It  is  necessary  to  either  improve  the  instrumentation 
suite  to  provide  the  desired  parameters  or  estimate  them  with  some  sort  of  observer.  The 
measured  vehicle  state  is  tmeas  =  \j>  q  r  z  §  Q  y\T •  The  missing  parameters, 
^missing  =  [M  v  T  ,  are  necessary  for  controller  implementation.  Laboratory  simulations 
use  a  full  state  nonlinear  model  for  vehicle  dynamics.  A  vehicle  observer  using  an  adapta¬ 
tion  of  that  simulation  calculates  missing  vehicle  parameters.  Measured  states  “reset”  the 
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corresponding  observer  parameters  every  control  cycle  so  that  calculated  parameters 
remain  reasonable.  A  cost  of  using  this  technique  is  the  added  computation  load  on  the 
vehicle  computer  for  control  purposes.  The  number  of  iterations  in  the  observer  is  reduced 
from  the  laboratory  simulator  to  prevent  impacting  the  vehicle  200  msec  control  cycle 
time.  This  does  not  adversely  affect  the  calculated  parameters. 

A  preferred  approach  to  state  estimation  is  an  instrumentation  package  providing  the 
desired  parameters,  reducing  the  computation  load.  Installation  of  a  Sontek  acoustic  dop- 
pler  velocimeter  (ADV)  is  planned. 

4.3  Speed  Dependent  Pitch  Limit 

When  trying  to  determine  the  significant  operating  parameters  during  slow  speed 
operations,  one  approach  is  to  examine  speed’s  role  in  the  dynamic  model.  The  most  direct 
effects  are  reduced  body  and  fin  lift  which  behave  according  to: 

Lf  =  0.5pA(Cl)U28  (4.1) 

Slower  operating  speeds  drastically  affect  depth/pitch.  Longer  duration  transients 
result  from  the  reduced  lift  and  torque  generated  by  the  fins  and  body.  In  the  pitch  plane, 
there  are  two  added  effects.  With  the  elevator  torque  having  to  counter  the  CG-CB 
moment  arm,  the  elevators  are  unable  to  sustain  as  large  a  pitch  angle  at  slower  speeds. 
Positive  buoyancy  limits  depth/pitch  control  the  most.  When  speed  drops  to  the  point 
where  hull  lift  is  insufficient  to  counter  the  buoyancy,  depth  becomes  uncontrollable. 

As  speed  changes,  pitch  angle  limits  change  as  listed  below  with  K=26  and  k=9. 

Q  limit  =  KU-k  (4.2) 
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While  varying  speed,  this  limit  keeps  the  desired  trajectory  physically  realizable  and 
attempts  to  avoid  stall  conditions  at  slower  speeds. 

4.4  Speed  Compensated  Trajectory  Generator 

A  trapezoidal  trajectory  generator  illustrated  in  Figure  4.1,  using  physically  realiz¬ 
able  accelerations  and  velocities,  generates  a  desired  trajectory  for  heading  and  depth 
changes.  These  trajectory  states,  with  the  vehicle  dynamics  model,  generate  “feedfor¬ 
ward”  terms  for  the  control  input  to  attain  low  tracking  error.  The  feedback  terms,  while 
working  to  keep  the  tracking  error  zero,  are  only  compensating  for  model  errors. 

With  slower  operating  speeds,  the  acceleration  and  velocities  obtainable  are  smaller. 
Trajectory  accelerations  and  velocities  are  reduced  accordingly  and  the  correction  magni¬ 
tudes  for  position  and  velocity  errors  are  greatly  reduced.  The  following  relations  limit  tra¬ 
jectory  accelerations  and  velocities  as  a  speed  function: 


\j fmax  —  U desired  *  \j fmax 

(4.3) 

Zmax  —  U desired  ■  sill  (0/im//) 

(4.4) 

\\fmax  —  2  •  U desired 

(4.5) 

Zmax  =  0.01  •  U  desired 

(4.6) 

Vehicle  open  loop  performance  at  varying  speedsfll]  determined  these  values.  The 
limits  are  within  the  vehicle  physical  limitations  so  during  a  transient,  full  fin  deflection  is 
not  required  to  achieve  an  impossible  desired  yaw  rate  or  pitch  angle  and  some  actuator 
range  is  available  for  corrections. 
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4.5  Minimizing  Actuator  Motions 

With  control  surface  torque  inputs  directly  dependent  on  speed,  the  larger  of  esti¬ 
mated  vehicle  speed  or  the  desired  speed  determines  fin  position  for  a  specific  control 
input. 

Lf  =  0.5pA(CZ)£/28  (4.7) 

A  5  =  - y~ - t  I  (4.8) 

1  0.5p  A(Cl)Uly 

This  approach  eliminates  excessive  actuator  motion  when  the  vehicle  is  slowing,  prevent¬ 
ing  unmodeled  dynamics  excitation  and  unnecessary  power  drain.  Additionally,  during 
start-up  transients  or  speed  increases,  the  initial  fin  motions  are  smaller,  reducing  fin  drag, 
allowing  the  vehicle  to  reach  operating  speed  quicker.  Figure  4.2  compares  two  depth 
changes  while  slowing.  In  the  transient  plotted  with  broken  lines,  excessive  actuator 
motion  drops  speed  until  stall  occurs  at  approximately  t=213,G50 

degrading  pitch  and  depth  keeping.  Using  a  speed  trajectory  generator  to  create  a 
smooth  transition  between  states  would  be  an  alternate  but  more  computationally  costly 
approach  to  minimize  actuator  motion.  This  example  demonstrates  how  excessive  actuator 
motion  driven  by  any  source  increases  drag  and  reduces  propulsion  efficiency. 
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elevator  angle 


pitch  vs  time 


speed  vs  time 


Figure  4.2  Comparing  Performance  With  and  Without  Limited  Elevator  Motion. 

With  elevator  position  based  on  a  lower  commanded  speed,  the  controller  over  corrects  for 
pitch  errors.  Large  elevator  oscillations  increase  fin  drag  and  slow  the  vehicle.  As  the  vehi¬ 
cle  slows,  hull  lift  is  lost,  stall  occurs  and  poor  depth  keeping  results. 


4.6  Robust  Sliding  Control  Simulation 

After  implementing  all  of  the  controller  features  discussed,  vehicle  performance 
simulations  aid  overall  and  individual  feature  evaluation.  The  first  task  is  to  evaluate  per¬ 
formance  with  multiple  speeds  during  the  same  mission.  The  mission  profile  used  is  a 
launch  from  zero  speed  to  a  maximum  1.5  meters/sec  and  then  two  step  decreases  to  0.7 
meters/sec.  Heading  changes  occur  simultaneously  with  speed  changes.  Next,  actuation 
filtering  benefits  are  shown  with  a  mission  profile  comparison.  The  vehicle  model  used  for 
simulations  has  fixed  vertical  fins  added  as  discussed  in  Chapter  5. 

4.6.1  Speed  Changes 

Figure  4.3  shows  heading  and  depth  performance  with  speed  changes  at  180  seconds 
and  360  seconds.  The  corresponding  change  in  heading  and  depth  rates  are  most  easily 
seen  as  longer  transients.  Figure  4.4  expands  the  original  time  scale  where  actual  and  tra¬ 
jectory  heading  are  indistinguishable.  The  pitch  and  depth  control  performance  is  not  as 
crisp  and  precise  as  heading  because  of  pitch  and  depth  inter-dependency.  A  pitch  and 
depth  control  expanded  view  is  also  shown  in  Figure  4.4. 

Because  the  “feed  forward”  control  input  terms  are  generated  based  on  the  desired 
trajectory  dynamics  instead  of  accumulated  errors,  as  in  a  typical  PID  controller,  the  cor¬ 
rections  that  are  need  to  keep  tracking  errors  small  are  greatly  reduced.  Acknowledging 
that  the  sliding  controller  feedback  is  not  multi-variable,  the  feed  forward  terms  do  pro¬ 
vide  a  multi-variable  control  input.  This  multi-variable  control  input  is  not  in  the  form 
generally  used  in  multi-variable  controllers  but  the  basic  tenets  are  there.  Individual  feed 
forward  input  magnitudes  as  well  as  the  role  feedback  plays  in  correcting  for  modeling 
errors  is  plotted  in  an  overall  and  expanded  mission  view  in  Figure  4.5  and  Figure  4.6. 


50 


1.5 


Speed  IUI  ] 


Pitch  [  act(-)  des( — )] 


Figure  4.3  Performance  With  Varying  Speed.  Transients  at  lower  speeds  require  longer 
times  to  complete  as  control  surface  lift  is  less.  Despite  large  changes  in  forward  speed 
vehicle  trajectory  tracking  is  excellent. 
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Yaw(-) .Trajectory  Yaw(— )  and  Ordered  Yaw(. ..) 


Rudder  Position  [  act(— )  des( - )] 


Figure  4.4  Expanded  View  of  Figure  4.3-  transient  at  1  m/sec.  One  transient  is  exam¬ 
ined  closely  for  a  better  look  at  trajectory  tracking.  Heading  and  depth  are  difficult  to  sep¬ 
arate  from  the  trajectory  in  the  plots.  Pitch  control  is  not  as  crisp  and  precise  as  heading 
because  of  the  inter-dependency  of  pitch  and  depth. 
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Feed  Fwd  Terms  —  swayrate(— ),hdgaccel(-.),sway( — ),hdg  vel(-) 


Control  Input.. Sum  of  Feed  Fwd  Terms(-.),Feedback( — ),Total(— ) 


Control  Input  -  Sum  of  Feed  Fwd  Terms(-.),Feedback( — ),Total(~) 


Figure  4.5  Control  Inputs  for  Mission  in  Figure  4.3.  Individual  feed  forward  terms  and 
their  sum  are  plotted  with  feedback  for  comparison.  Individual  feed  forward  terms  are  dif¬ 
ficult  to  see  in  this  plot  because  of  the  time  scale.  During  the  maneuvers,  feed  forward 
terms  are  primarily  responsible  for  tracking  performance  as  feedback  is  small. 
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Control  Input  Control  Input  Control  Input  Control  Input 
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Figure  4.6  Control  Inputs  for  Transient  in  Figure  4.4-  transient  at  1  m/sec.  A  closer 
look  shows  the  relative  importance  of  each  feed  forward  term  and  the  relatively  small  role 
feedback  is  playing  in  the  tracking  performance. 


4.6.2  Eliminating  Noise  Driven  Actuator  Motion 

Sensor  noise  driving  excessive  fin  motion  is  a  significant  concern.  Control  activity  at 
5  Hz,  the  basic  control  cycle  frequency,  is  an  unnecessary  power  drain  on  the  vehicle. 
Using  alternate  output  modes  built  in  to  the  sensor  to  reduce  noise  were  unsuccessful  and 
changing  sensors  is  fiscally  prohibitive.  Considering  the  already  heavy  state  estimator 
computational  load,  a  rudimentary  filter  utilizes  physical  limitations.  Gear  backlash  in  the 
fin  actuators  is  estimated  to  be  0.5  degrees  and  total  actuator  drivetrain  backlash  measures 
a  minimum  5  degrees.  Limiting  actuator  repositioning  to  increments  greater  than  0.5 
degrees,  effectively  eliminates  sensor  noise  during  simulation  with  minimal  performance 
cost.  The  only  real  loss  in  performance  is  during  actuator  positioning  that  further  advances 
the  fin  and  does  not  involve  “gear  slop”.  Fin  activity  decreases  from  5  Hz  to  approximately 
1  Hz  with  very  satisfactory  performance.  Increased  lift  forces  make  limiting  actuator 
motion  more  detrimental  at  higher  speeds.  The  threshold  for  fin  motion  varies  inversely 
with  speed  as  follows: 

hallowed  =  0.5  •  •  —  (4.9) 

The  next  figures  show  how  the  actuator  filter  reduces  motion  without  adversely 
impacting  performance.  In  Figure  4.7,  with  both  missions  plotted  on  the  same  axes,  there 
is  little  difference  on  the  large  scale  as  well  as  the  Figure  4.8  expanded  scale.  It  is  neces¬ 
sary  to  examine  expanded  transients  in  Figure  4.9  and  Figure  4.10  to  see  the  benefits 
gained  in  actuator  motion. 
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Figure  4.7  Comparing  Performance  With  and  Without  Actuator  Filter.  Separating  the 
plots  of  two  missions  on  this  time  scale  is  difficult  because  the  performance  is  essentially 
identical.  A  closer  look  is  taken  in  the  following  figures. 


Elevator  Position  (degrees) 


Figure  4.8  Expanded  Portion  of  Actuator  Filter  Comparison  -  one  transient  at  1  m/ 

sec.  Again,  two  plots  are  difficult  to  see  when  looking  at  only  one  transient,  because  of  the 
virtually  identical  performance.  This  transient  is  expanded  again  in  the  next  two  plots. 


Yaw(-), Trajectory  Yaw(- .— )  and  Ordered  Yaw(-.) 


Figure  4.9  Expanded  Portion  of  Actuator  Filter  Comparison  -  beginning  of  transient. 

Here  the  difference  between  trajectory  and  actual  depth  and  heading  can  be  seen  but  the 
two  vehicle  plots  are  essentially  identical.  What  can  be  seen  is  just  how  much  less  control 
surface  activity  is  needed  for  the  same  tracking  performance. 
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Yaw(-), Trajectory  Yaw(— )  and  Ordered  Yaw(— .) 


Figure  4.10  Expanded  Portion  of  Actuator  Filter  Comparison  -  end  of  transient.  As 

the  new  commanded  heading  and  depth  are  reached,  less  control  surface  activity  still  gives 
good  tracking  performance.  Performance  initially  may  seem  poor  because  of  the  vertical 
axis  scaling. 
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4.6.3  Adaptive  Sliding  Control 

Improving  performance  over  time  is  the  benchmark  for  evaluating  adaptation.  If  the 
feedback  magnitude  decreases  over  time  then  the  adaptation  scheme  works.  Empirically 
selected  gains  allow  slow  adaptation  to  avoid  oscillations.  The  gains  are  kept  high  to  allow 
parameter  estimations  to  slowly  converge  to  the  model  parameters  for  a  continual 
improvement  in  performance.  The  gain  matrixes  used  for  adaptation  are  given  in  (4.10) 
and  (4.11). 


rv  =  [o.05‘  0.3  0.05  0.05  O.O5]  (4'10) 

Fe  =  [o.4  0.4  0.4  0.04  '  0.1  0.04]  (4'n) 

Adapted  parameter  bounds  of  +/-30  percent  from  initial  estimated  values  prevent 
instability  due  to  parameter  drift  or  unmodeled  disturbances.  This  attempts  to  guarantee 
some  minimum  performance  level.  Adaptation  critics  complain  about  run  away  adapta¬ 
tion.  With  an  accurate  initial  hydrodynamic  parameter  determination  or  estimation,  and 
then  limiting  subsequent  physical  alterations,  excellent  performance  is  possible  without 
excessive  adaptation  causing  instabilities. 

For  the  adaptation  comparisons,  heading  and  depth  changes  are  made  at  a  constant 
speed.  The  first  simulations  use  updated  model  parameters  with  fixed  vertical  fins  present 
to  increase  the  vehicle’s  directional  stability.  The  basis  for  these  fins  is  the  subject  of 
Chapter  5.  Initial  parameters  used  by  the  controller  differ  from  those  used  in  the  full  state 
simulation  by  30%,  forcing  some  modeling  errors.  Even  with  these  errors,  feedback  mag¬ 
nitude  is  insufficient  for  evaluation.  To  introduce  additional  errors,  the  model  parameters 
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used  by  the  controller  are  those  for  the  vehicle  without  the  vertical  fixed  fins  while  the 
simulation  model  has  the  fins  in  place.  The  model  errors  are  then  large  enough  for  conve¬ 
nient  comparisons  and  are  shown  in  Figure  4.1 1  through  Figure  4.15.  Here  again,  the  dif¬ 
ference  in  performance  is  difficult  to  discern  on  a  large  scale.  Figure  4.14  and  Figure  4.15 
expands  the  heading  change  at  t  =  90  seconds  for  a  closer  look. 

Figure  4.15  isolates  the  feedback  during  three  turns  in  the  same  direction,  at  the 
same  speed  for  examination.  The  dashed  plot  shows  the  feedback  without  adaptation  tak¬ 
ing  place.  The  solid  plot  presents  the  decrease  in  feedback  with  adaptation  present  and 
indicates  adaptation  is  improving  performance.  The  lower  plot  in  Figure  4.15  shows  a 
feedback  close-up  during  the  three  turns. 

4.7  Summary 

Model  based  adaptive  sliding  controller  implementation  with  a  full  state  nonlinear 
estimator  provides  feed  forward  control  inputs  that  allow  accurate  desired  trajectory  track¬ 
ing.  Using  the  detailed  controller  model  and  a  trajectory  generator  that  compensates  for 
speed,  excellent  performance  throughout  the  vehicle  speed  range  is  possible  without  con¬ 
troller  retuning.  Feed  forward  terms  generated  by  the  trajectory  generator  and  vehicle  state 
estimator  achieve  high  tracking  accuracy.  These  feed  forward  terms  provide  multi- variable 
control  inputs  based  on  the  desired  flight  path  rather  than  error  accumulation.  Addition¬ 
ally,  an  actuation  filter  prevents  actuator  motion  of  a  smaller  magnitude  than  the  observed 
actuator  drivetrain  gear  backlash.  Any  impact  on  performance  using  this  filtering  approach 
is  minimal  and  actuator  motion  is  reduced  from  5  Hz  to  at  least  1  Hz.  Model  parameter 
adaptation  allows  the  continuing  performance  improvement  as  time  progresses  by  refining 
the  parameter  estimates.  These  revised  parameter  estimates  and  the  resulting  performance 
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improvement  manifest  themselves  as  a  reduction  in  the  overall  feedback  required  to  com¬ 
pensate  for  modeling  errors.  The  net  gain  with  this  controller  is  improved  performance 
over  a  range  of  operating  speeds  with  less  actuator  motion  required. 
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Figure  4.12  Adaptation  of  Heading  Model  Terms.  Original  parameters  used  by  the  con¬ 
troller  are  plotted  with  the  updated  estimates  to  show  how  adaptation  changes  the  parame¬ 
ter  values.  These  plots  show  a  gradual  convergence  of  the  estimates.  Note  that  excellent 
tracking  performance  is  achieved  with  modeling  errors  present.  The  large  feedback  input 
in  Figure  4.14  is  an  indication  of  the  large  modeling  errors  present. 
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Ahat[3] 


Control  Input  Rudder  Position  (degrees)  Heading  (degrees) 


Yaw(-), Trajectory  Yaw(— )  and  Ordered  Yaw(— .) 


Feed  Fwd  Terms.,  sway  rate  ( — ),hdgaccel(-),sway(-  &  — ),hdg  vel(—  &  — ) 


Control  Input. .Sum  of  Feed  Fwd  Terms( —  &  — .),Feedback(—  &  — ),Total(—  &  — .) 


Figure  4.14  Expanded  Portion  of  Figure  4.11  -  heading  change  with  control  inputs. 

Small  differences  between  the  two  plots  illustrate  that  tracking  performance  can  be  good 
even  with  modeling  errors  .With  the  larger  modeling  errors  tracking  performance  suffers. 
Vehicle  plots  with  adaptation  are  shown  as  solid  vice  dashed  lines. 


66 


Control  input  Control  input 


200  3 

time 


100  150  200  250  300  350  400  450  500 

time  (secs) 


Figure  4.15  Feedback  for  Heading  Changes.  Improving  estimates  of  model  parameters 
means  that  feed  forward  terms  are  doing  more  of  the  work  and  less  feedback  is  needed  for 
good  tracking  performance. 


Chapter  5 


Directional  Stability 

Initial  testing  at  Sea  Grant  Laboratory  proves  the  vehicle  is  unstable  in  the  yaw  plane 
or  that  it  is  directionally  controls-fixed  unstable.  Using  Hover’s  linear  model  and  the 
hydrodynamic  parameters  determined  at  Draper  Laboratory [11],  the  transfer  function 
poles  predict  vehicle  stability  in  pitch  and  instability  in  yaw.  The  difference  between  the 
pitch  and  yaw  planes  is  the  presence  of  the  CG-CB  moment  arm,  Eqs.  (5.5)  and  (5.13), 
which  provides  the  stability  in  the  pitch  plane.  The  pitch  and  yaw  plane  models  use  the 
most  accurately  determined  hydrodynamic  values  to  show  predicted  stability. 


Model  Parameters 

Maw 

396.81  Kg 

m 

339.26  Kg 

Mav 

396.81  Kg 

xb 

.0024  m 

Jyy 

70.83  Kg 

xw 

.00243  m 

Jamy 

30  Kg 

xfin 

-1.0691  m 

Jzz 

70.83  Kg 

zg 

0.082  Kg 

Jamz 

30  Kg 

zb 

0.0  m 

Zuw 

-137.6N 

xma 

-0.161m 

Zuq 

-181.2N 

Afin 

.0299  m 

Muw 

-236.Nm 

Cl 

3.3921  m 

-78.6Nm 

B 

1173  N 

£ 

-137.6  N 

W 

1173  N 

ESI 

-181.2  N 

Nuv 

-181.2Nm 

Nut 

-78.6  Nm 

Table  5.1:  Model  Parameters 
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5.1  Model  for  Pitch 


7e*  = 

Aqx  +  Beti 

(5.1) 

£  = 

[w  q  e] 

(5.2) 

u  — 

[Mr 

(5.3) 

/e 


m  -f  Maw  -  mxc  -  XmaMaw  0 

-  mXG  -  XmaMaw  Jyy  +  J  a  my  0 
0  0  1 


(5.4) 


Ae 


ZuwU  ~  Cfin  U  -  Zuq U  +  mU  +  Cfin  UXfin  B-W 
-MuwU  +  CfinUXfin  MuqU  -mUXG-CfinUXfin2  zbB-zgW 
0  cos<j>  0 


(5.5) 


-CfinU2  W-B 

XfinCfinU  XbB—XwW 
0  0 


(5.6) 


Cfin 


1 


P  AfinCl 


(5.7) 


Qpoles  — 


-2.0501 

-0. 10355  +  1 0.34045 
-0.10355-/0.34045 


(5.8) 


5.2  Model  for  Yaw 


IyX  =  Av/X  + 


(5.9) 
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Xy  =  [v  r 


(5.10) 


u  =  hr 


(5.11) 


h 


m  +  Mav  mXG  +  XmaMav  0 
mXG  +  XmaMav  Jzz  +  Jamz  0 
0  0  1 


(5.12) 


Ay 


Yuv U-CfinU  YurU-mU-  Cfin UXfin  0 
NuvU  —  CfinUXfin  NurU  —  mUXG  —  CfinUXfin4  0 
0  cos  <j)  0 


(5.13) 


B\\r 


CfinU 2 

2 

XfinCfinU 

0 


(5.14) 


\\fpoles  “ 


0 

-2.4441 

0.18688 


(5.15) 


5.3  Hydrodynamic  Stability 

These  poles,  Eq.  (5.15),  show  yaw  plane  open  loop  instability.  Two  fundamental 
ideas  are  pursued  when  examining  how  to  reduce  power  consumption  associated  with  con¬ 
trol  activity.  Chapter  4  discusses  the  first,  the  elimination  of  noise  driven  actuator  motion. 
The  second  is  the  directional  stability  being  determined  by  the  closed  loop  control  system. 
With  constant  rudder  motion  required  to  keep  the  bow  from  being  blown  off  the  intended 
course  by  any  disturbance,  an  opportunity  to  reduce  the  power  required  for  propulsion/ 
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steering  purposes  exists.  If  the  vehicle  controls-fixed  stability  could  be  improved,  poles  in 
the  left  half  plane  or  at  least  close  to  zero,  steering  with  less  rudder  activity  results.  Addi¬ 
tionally,  with  the  marginally  valid  assumption  that  roll  and  pitch  can  be  decoupled 
depending  on  the  trim  conditions,  less  rudder  action  translates  into  less  rolling.  Less  roll¬ 
ing  means  less  cross-coupling  to  pitch  and  depth  control  requires  less  elevator  action. 
Hence,  benefits  from  improving  directional  stability  are  gained  in  both  pitch  and  yaw 
planes. 

In  [7],  directional  stability  analysis  points  out  that  (5.16)  determines  stability. 


C  =  Y\N'r-N’v(rr-M')>  0  (5.16) 

The  terms  in  this  expression  are  non-dimensionalized  as  indicated. 


Y\ 


Y'r 


N'v 


N'r 


Yv 

YuvU  _ 

(-137.6K1.45) 

U2V 

2 

U2v 

2 

C“) 

2.152(1.45) 

Yr 

YurU  _ 

-181.2(1.45) 

%L*V 

2 

gLV 

2 

C“) 

2.153(1.45) 

Nv 

NuvU 

-181.2(1.45) 

eLV 

2 

-  |t*V  " 

A028> 

V  2  j 

|2.153(1.45) 

Mr 

_  NurU  _ 

-78.6(1.45) 

\lAV  2lV  ^i^§j2.154(1.45) 


-5.79  •  10  2 


-3.55  •  10  2 


-3.55  •  10  2 


-7.16  ■  10'3 
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M'  = 


M 


-78.6 


=  -7.16-10  3 

§*’ 


So  the  expression  becomes 

C  =  (4.15  •  10  4)  -  (-3.55  •  10  2)(-3.55  •  10'2-(-7.16  •  10-3)) 


C  =  -5.91  •  10^ 


and  predicts  instability. 

For  stability,  the  stem  must  present  more  resistance  to  crossflow  and  yawing.  In  the 
presence  of  an  athwartship  velocity,  the  stem  must  dominate  so  the  induced  torque  from 
crossflow  drag  swings  the  bow  in  the  athwartship  velocity  direction.  In  the  presence  of  a 
yaw  velocity  the  stem  must  again  dominate  so  that  the  induced  torque  resists  the  yawing 
motion  more  in  the  stem  than  the  bow.  The  method  chosen  for  improving  the  crossflow 
drag  and  its  moment  arm  for  the  afterbody  versus  the  forebody  is  adding  two  fixed  vertical 
fins,  one  on  top  and  one  below,  just  forward  of  the  rudder.  Figure  5.1  shows  the  hull  form 
with  the  fixed  vertical  fins  installed. 

Adding  two  fins  the  same  size  as  the  existing  rudder  surfaces,  changes  predictions 
for  vehicle  directional  stability.  This  prediction  is  made  by  adding  the  fixed  vertical  fins 
effect  to  the  hydrodynamic  values  and  determining  the  transfer  function  poles.  Predicted 
poles  with  additional  fins  are 


\\fpoles  = 


0 

-2.7557 

-0.0998 


With  no  poles  in  the  right  half  plane,  stability  is  predicted.  Figure  5.2  is  a  pole  plot  show- 
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ing  the  vertical  fins  effect.  A  physical  change  unmodeled  for  this  study  is  the  docking 
latch.  The  latch  increases  the  bow  effects  and  reduces  fixed  vertical  fins  effectiveness. 
Actual  results  are  not  expected  to  be  as  good  as  predicted. 


Plan  view  of  vehicle  with  fins,  dimensions  in  inches 


Figure  5.1  Hull  Form  with  Fixed  Fins  and  Latch.  Two  fixed  vertical  fins  forward  of  the 
rudder  stabilizes  the  vehicle  in  the  horizontal  plane 
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Yaw  Plane  Pole  Plot  -  w/o  fins(X),  w/fins(0) 


Figure  5.2  Yaw  Plane  Pole  Plot  Showing  Effect  of  Vertical  Fins.  Adding  fixed  vertical 
fins  moves  the  one  unstable  pole  into  the  left  half  plane  predicting  controls-fixed  direc¬ 
tional  stability. 

5.4  Dieudonne1  Spiral  Maneuver 

With  improved  stability  predictions,  the  Dieudonne’  Spiral  Maneuver  is  chosen  to 
determine  the  added  fins  effect  on  stability.  As  discussed  in  [7]  this  definitive  ship  trial 
maneuver  identifies  the  directional  stability  characteristics.  The  maneuver  is  performed 
by: 


1.  Steadying  on  a  course  and  speed  for  about  a  minute.  Once  a  steady  speed  is  estab- 
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lished,  no  change  to  thruster  rpm  is  made. 

2.  The  rudder  is  put  over  to  about  15  degrees  and  held  until  a  constant  yaw  rate  is 

achieved. 

3.  The  rudder  is  then  reversed  by  indexing  in  5  degree  increments,  achieving  steady 

yaw  rates  at  each  rudder  position. 

4.  The  rudder  is  reversed  again  in  the  same  manner,  finishing  with  the  rudder  at  15 

degrees  to  the  original  side. 

The  steady  yaw  rates  as  a  rudder  angle  function  are  then  examined.  If  the  yaw  rates 
plot  as  a  single  fine  going  from  port  to  starboard  rudder  and  back  again,  the  ship  is  straight 
fine  controls-fixed  stable.  If  the  plot  forms  a  hysteresis  loop  the  ship  is  unstable.  The  vehi¬ 
cle  may  turn  with  the  rudder  amidships  and  no  external  disturbances  acting  or  having 
acted  in  the  recent  past.  For  the  stable  ship,  only  one  turning  rate  for  a  given  rudder  posi¬ 
tion  exists,  as  opposed  to  the  unstable  situation  where  several  yaw  rates  are  possible  for 
rudder  positions  in  a  neighborhood  about  zero.  In  the  unstable  neighborhood  about  zero, 
the  vehicle  can  turn  against  its  rudder.  With  starboard  rudder  on,  the  steady  state  yaw  rate 
could  be  to  port.  Figure  5.3  illustrates  a  rudder  angle-yaw  rate  plot  for  a  stable  and  unsta¬ 
ble  situation.  Figure  5.4  shows  simulated  performance  with  an  updated  model  to  include 
the  vertical  stationary  fins. 
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Port  Rudder(degrees)  Stbd 


Port  Yaw  Rate(deg  rees/sec)  Stbd 

Figure  5.3  Typical  Dieudonne’  Spiral  Maneuver  Plot. 


For  ships  that  are  dynamically  and  geometrically  symmetric  about  the  xz-plane, 
these  rudder  curves  are  symmetric  about  zero  rudder  angle  and  zero  yaw  rate.  An  odd 
number  of  propellers  precludes  dynamic  symmetry  in  the  xz-plane  and  displaces  the  rud¬ 
der  curve  to  one  side  or  the  other  depending  on  screw  rotation  direction. 
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SIMULATED  DIEUDONNE  PLOT  [  (*-)  w/o  fins,  (x-.-)  w/fins  ] 


Figure  5.4  Simulated  Dieudonne  Plot. 


Figure  5.5  shows  a  shifted  rudder  curve  as  well  as  the  initial  directional  instability 
and  the  improvement  after  adding  fixed  fins.  To  obtain  a  closed  hysteresis  loop  for  the 
vehicle  with  no  fins,  in  Figure  5.5,  more  data  for  a  larger  turning  rate  to  port  must  be  col¬ 
lected.  A  higher  y  to  port  would  close  the  hysteresis  loop.  Although  adding  fixed  fins 
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improves  the  directional  stability,  additional  testing  is  necessary  to  determine  absolute  sta¬ 


bility. 


DIEUDONNE  PLOT  [  (*— )  w/o  fins,  (x-)  w/fins  ] 


Figure  5.5  Odyssey  Dieudonne’  Spiral  Maneuver  Plot.  Real  vehicle  data  indicates  ver¬ 
tical  fins  stabilized  the  vehicle  in  the  horizontal  plane.  The  hysteresis  loop  is  not  closed 
because  of  the  dynamic  offset  of  one  propeller.  The  loop  is  expected  to  close  at  a  higher 
port  yaw  rate. 


Figure  5.6  compares  data  for  vehicle  roll  and  elevator  motion  during  the  spiral 
maneuver.  As  expected,  the  rolling  and  elevator  motion  with  the  fixed  vertical  fins  are  sig¬ 
nificantly  quieter.  The  elevator  plot  shows  not  only  the  actuator  motion  reduction  but  the 
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presence  of  noise  in  the  control  input. 


Roll  Plot(dashed  lines/no  fins) 


Time  (seconds) 


Elevator  Plot  (dashed  lines/no  fins) 
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Figure  5.6  Roll  and  Elevator  Motion  for  Fixed  Fin  Evaluation.  During  the  spiral 
maneuver,  performance  with  vertical  fins  shows  quieter  vehicle  dynamics.  Less  rolling 
means  rudder  effects  on  pitch  are  less.  As  a  result  less  elevator  motion  is  required  for 
depth  keeping.  Note  that  with  fins  installed  sensor  noise  effects  are  seen  in  the  elevator 


motion. 

Another  characteristic  altered  by  the  hull  modification  is  the  vehicle  rolling  into  the 
turn.  Without  fixed  vertical  fins,  a  well  defined  shift  in  roll  is  seen  as  the  vehicle  yaw  rate 
changes  direction.  Figure  5.7  shows  the  change  in  roll  angle  is  less  pronounced  with  the 
fixed  fins  installed.  This  indicates  a  quieting  of  vehicle  dynamics. 


Roll  Plot(dashed  lines/no  fins) 


Figure  5.7  Shift  in  Roll  Angle  at  Zero  Yaw  Rate.  With  vertical  fins  installed  the  vehicle 
rolls  into  the  turns  less.  This  plot  shows  how  roll  shifts  as  the  turn  changes  direction.  In  a 
homing  situation,  where  the  vehicle  is  hunting  back  and  forth  to  null  a  USBL  received 
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Rudder  Position  (degrees)  Heading  (degrees)  Rudder  Position  (degrees)  Heading  (deg 


bearing,  roll  magnitude  would  be  reduced. 


Yaw  w/fins(— )  &  w/o  fins( — ),Trajectory  Yaw(-.-)  and  Ordered  Yaw(...) 


Figure  5.8  Comparison  of  Simulated  Heading  Performance  with  Vertical  Fins. 

Improved  performance  with  vertical  fins  is  demonstrated  in  this  transient.  Directional  sta- 
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bility  provided  by  the  fixed  vertical  fins  means  better  performance  with  less  actuator 
motion. 

Figure  5.8  shows  a  maneuver  for  a  vehicle  with  and  without  fins.  Here  the  increased 
rudder  to  make  the  heading  change  is  seen  clearly.  The  expanded  plot  shows  the  heading 
instability  for  the  vehicle  without  fins  and  the  increased  rudder  action  to  maintain  heading. 

5.5  Summary 

Improving  the  vehicle  controls-fixed  directional  stability  reduces  control  surface 
activity  for  both  the  rudder  and  elevator.  Adding  fixed  vertical  fins  to  the  afterbody  stabi¬ 
lizes  the  vehicle  in  the  horizontal  plane.  Reduced  rudder  motion  for  steering  is  the  direct 
benefit.  Less  obvious  benefits  are  reduced  rolling  and  pitching  leading  to  less  elevator 
motion  for  depth  control.  Quieter  vehicle  dynamics  means  energy  savings  from  reduced 
fin  drag  and  actuator  motion. 
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Chapter  6 


Conclusions  and  Future  Work 

Improvements  in  the  three  areas  examined  achieved  marked  improvement  in  vehicle 
performance.  Analysis  of  each  change  shows  that  the  modifications  made  as  a  whole,  or 
selective  changes  made  incrementally,  provide  better  vehicle  performance  with  less 
energy  expended.  This  section  presents  conclusions  concerning  each  modification.  Addi¬ 
tionally  the  proposed  modifications  are  ordered  in  easiest  to  most  involved  as  far  as  imple¬ 
mentation  difficulty. 

6.1  Actuator  Filtering 

A  rudimentary  filter  based  on  the  physical  gear  backlash  limitation  eliminates  exces¬ 
sive  actuator  motion  driven  by  sensor  noise.  With  little  or  no  noticeable  effect  on  perfor¬ 
mance,  the  filter  reduced  actuator  motion  from  5  Hz  to  less  than  1  Hz.  This  analysis  uses 
simulations,  but  initial  actuator  motion  evaluation,  gear  backlash  and  sensor  noise  levels 
indicates  similar  results  are  possible  with  the  vehicle. 

Filter  implementation  is  by  far  the  easiest  as  it  requires  a  software  change  with  eval¬ 
uations  having  minimal  impact  on  planned  operations.  Determining  the  optimum  filter  set¬ 
tings  can  be  done  gradually  and  evaluated  quickly  during  the  normal  course  of  any  vehicle 
deployment  preparations.  Decreasing  actuator  motion  from  5  Hz  to  less  than  1  Hz  greatly 
reduces  the  power  consumed  for  vehicle  control.  Working  amp-hour  sensors  allow  either  a 
one  time  power  budget  analysis  or  continual  evaluation.  Benefits  from  each  modification 
presented  here  could  be  quantified  and  further  evaluated. 
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6.2  Directional  Stability 

Starting  with  an  initial  controls-fixed  directional  instability,  adding  two  fixed  vertical 
stationary  fins  achieves  stable  open-loop  dynamics.  These  fins  greatly  reduce  actuator 
motion  necessary  to  maintain  a  steady  course.  With  inherent  rolling  of  the  vehicle,  rudder 
cross  coupling  causes  pitching  that  can  not  be  eliminated,  only  reduced.  Less  rudder 
motion  results  in  less  pitching  and  rolling  and  a  corresponding  reduction  in  elevator 
motion.  Comparing  the  costs  and  benefits,  the  drag  reduction  from  less  actuator  motion  far 
outweighs  any  increased  drag  from  the  addition  of  fixed  fins.  Less  control  surface  drag, 
together  with  the  power  savings  from  reduced  actuator  motion  in  both  horizontal  and  ver¬ 
tical  planes  achieves  even  larger  reductions  in  power  consumption  by  propulsion  and  vehi¬ 
cle  control. 

This  modification  requires  approximately  $150  of  material  for  the  two  fins  and  fin 
boxes  for  mounting.  Fin  installation  requires  minor  machining  and  hull  modifications  that 
could  be  accomplished  for  one  vehicle  in  eight  man-hours.  Per  vehicle  man-hour  costs  are 
reduced  if  making  multiple  vehicle  modifications.  There  are  no  added  operational  require¬ 
ments  for  evaluations.  Improvements  obtained  in  vehicle  control  alone  make  this  modifi¬ 
cation  worthwhile.  Once  made,  improvements  in  vehicle  performance  are  likely  during 
homing  as  well.  With  less  pitching  and  rolling  during  homing,  less  data  scatter  from  the 
Ultra  Short  Base  Line  Navigation  system  and  a  corresponding  improvement  in  homing 
performance  is  probable.  Other  imaging  packages  such  as  side  scan  sonar  benefit  greatly 
from  a  stable  platform.  Reduced  vehicle  motion  provides  side  scan  sonar  images  with 
greater  clarity  and  higher  resolution. 
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6.3  Controller 


An  adaptive  model  based  sliding  controller  designed  and  implemented  in  simulation 
shows  robustness  to  speed  changes  and  modeling  errors.  With  model  parameter  adapta¬ 
tion,  an  already  high  performance  level  improves.  The  ability  to  enjoy  consistent  perfor¬ 
mance  without  expending  significant  man-hours  following  minor  instrumentation  changes 
is  certain. 

Sliding  adaptive  controller  implementation  requires  software  integration  which 
should  be  a  minimal  effort  as  the  controller  has  been  kept  current  with  software  revisions. 
Once  installed,  initial  controller  tuning  is  estimated  to  take  a  couple  days  of  vehicle  opera¬ 
tions.  This  initial  controller  tuning  is  expected  to  be  a  one  time  cost  as  subsequent  opera¬ 
tions  will  be  free  of  this  effort.  Adaptation  can  be  turned  on  and  off  as  desired.  Small 
incremental  changes  in  the  model  parameters  can  improve  performance  without  impacting 
vehicle  operations.  After  an  adequate  evaluation  period  following  configuration  changes, 
new  model  parameters  can  be  fixed  and  adaptation  disabled  if  desired. 

The  changes  in  operational  scenarios  possible  with  the  sliding  adaptive  controller  are 
enormous.  Operating  at  a  speed  that  maximizes  propulsion  efficiency  during  transits  and 
then  changing  speed  to  optimize  sensor  performance  during  sampling  can  be  a  routine 
matter.  Other  operational  possibilities  are: 

-maintaining  a  constant  speed  over  ground  in  changing  current  conditions  during 
sidescan  sonar  imaging.  Constant  image  resolution  could  be  maintained. 

-  “Soft  Docking”  or  slowing  the  vehicle  during  a  homing  run  to  minimize  impact 
velocity.  This  reduces  mechanical  shock  to  electronic  equipment  inside  the  spheres 
as  well  as  the  chance  of  latch  component  failure  from  repeated  docking  impacts  in 
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the  colder  ocean  waters.  Improved  final  homing  maneuver  effectiveness  means  fewer 
“misses”. 

-  Increased  maneuverability  during  bottom  following  and  obstacle  avoidance  behav¬ 
iors. 

There  are  bound  to  be  many  other  changes  in  operational  scenarios  and  vehicle  behaviors 
possible  once  operators  realize  that  constant  speed  missions  are  no  longer  a  restriction. 

After  the  Sontek  Acoustic  Doppler  Velocimeter  installation,  the  computation  inten¬ 
sive  estimator  can  be  eliminated.  Accurate  multi-axis  velocities  from  the  ADV  void  the 
necessity  for  state  estimation.  This  improvement  will  be  practical  if  the  sensor  accuracy  is 
high  and  noise  level  sufficiently  low. 
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Appendix 


Controller  Equations 


A.l  Parameter  Initialization 

#define  D2R(d)  (d*PI/180.0) 

#define  R2D(r)  (r*  180.0/PI) 

double  GAMMA[ll]={5e-2,3e-l,5e-2,5e-2,5e-2,le-l,le-2,2e-l,5e-3,le-l,le-2}; 
static  double  Ahatdot[ll]={0,0, 0,0,0,  0,0,0,0,0}; 

/*THIS  IS  GOOD  FOR  NO  FIN  VEHICLE*/ 

static  double  Ahat[l  1  ]= { -6.3062e+01 , 1 .0083e+02, 1 .2563e+02, 1 ,3884e+02, 
0,6.3062e+01,  1.0083e+02  ,-1.8133e+02,  7.9336e+01,  9.6186e+01,1.0  }; 


/*THIS  IS  GOOD  FOR  FINNED  VEHICLE*/ 

/*static  double  Ahat[ll]={-1.0299e+02,1.0083e+02,1.4357e+01,2.5775e+02, 
0,6.3062e+01, 1.0083e+02  ,-1.8133e+02,  7.9336e+01,  9.6186e+01,1.0  };  */ 


/****  *******************************  *********************************/ 


/*  THIS  PROGRAM  WORKS  WITH  DEGREES,  NOT  RADIANS  .  VALUES  FOR 
ANGLES  WITH  A  FEW  EXCEPTIONS  ARE  STORED  IN  THE  DATA  STRUCTURE 
AS  RADIANS  BUT  ARE  CONVERTED  TO  DEGREES  FOR  MOST  CALCULA¬ 
TIONS.  FOR  THE  INPUT  CALCS,  TERMS  WITH  DEGREES  HAVE  TO  BE  CON¬ 
VERTED  BACK  TO  RADIANS  FOR  CONSISTENT  UNITS  */ 


/*  load  present  vehicle  state  */ 

headingjrate  =R2D(  sensor(M_YAW_RATE) );  /*  IN  DEGREES/SEC  */ 

des_speed  =  vvp->c[0]. speed [0];/*  IN  METERS/SEC  */ 

speed  =  sensor(E_SPEED); 

pitch_limit=26  *speed-9.0; 

pitch_limit_traj=26*des_speed-9.0; 

maxdr  =  sin(D2R(pitch_limit_traj))*des_speed  ;/*  IN  METERS/SEC  */ 

pitch_rate  =  R2D(sensor(M_PITCH_RATE)  );  /*  IN  DEGREES/SEC  */ 

depth  =  sensor(M_DEPTH);  /*  IN  METERS  */ 

depth_rate  =  (depth-old_depth)/dt;  /*  IN  METERS/SEC  */ 

c_depth  =  sensor(C_DEPTH);  /*  IN  METERS  */ 

sway  =  sensor(E_V);/*  IN  METERS/SEC  */ 

sway_rate  =  sensor(E_VD);  /*  IN  METERS/SECA2  */ 

heave  =  sensor(E_W);/*  IN  METERS/SEC  */ 

heave_rate  =  sensor(E_WD);  /*  IN  METERS/SEC A2  */ 

roll  =  R2D(sensor(M_ROLL) );  /*  IN  DEGREES  */ 
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pitch  =  R2D(sensor(M_PITCH)) ;  /*  IN  DEGREES  */ 

heading  =  R2D(sensor(M_YAW)); 

sheading  =R2D(  sensor(C_HE ADING)) ;  /*  IN  DEGREES  */ 

lambda_theta  =  sensor(  LAMBDA_THETA ); 

k2_p  =  sensor(  K2_P); 

lambda_p  =  sensor(  LAMBDA_P ); 

lambda_z  =  sensor(  LAMBDA_Z  ); 

lambda_h  =  sensor(  LAMBDA_H ); 

kl_h  =sensor(  K1_H ); 

maxhr  =  R2D(des_speed  *sensor(UO_MAX_HR));  /*  IN  METERS/SEC  */ 
amax_depth=.01  *des_speed  ;  /*  IN  METERS/SEC A2  */ 
amax_hdg  =2*des_speed  ;/*  IN  DEGREES/SECA2  */ 
rtime  =  sensor(  M_PRESENT_TIME ); 
dt  =  rtime  -  last_time; 
if  (dt  >  .3)  dt  =  .3  ; 
if  (rtime  <=  2.*dt){ 
ctr=0; 

while  (ctr  <=9){ 

Ahat_max[ctr]  =  Ahat[ctr]*1.2; 

Ahat_min[ctr]  =  Ahat[ctr]*.8; 
ctr  4=1; 

} 

init_traj_z(depth,c_depth,amax_depth,  maxdr,rtime,&depth_traj); 

depth_traj.x  =  depth ; 

depth_accel=0; 

depth_rate=0; 

} 

if  (rtime  <=  2.*dt){ 

init_traj_hdg(heading,c_heading  ,amax_hdg,  maxhr, rtime,&hdg_traj) ; 
hdg_traj.x  =  heading  ; 

} 


A.2  Pitch/Depth  Control 

/* - 

I 

I  Adaptive  controller  for  Depth  control 
I 

IPhil  LeBas  7/96 


.*/ 
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/*  added  calculation  for  the  adaptive  controller  */ 
/* —  Do  dynamic  control  of  depth....  */ 


/*  check  to  see  if  the  commanded  depth  has  been  changed 
-  if  so  compute  the  new  trajectory  */ 

if  (old_c_depth  !=  c_depth)  init_traj_z(depth,c_depth,amax_depth, 
maxdr,rtime,&depth_traj ) ; 


/************************************************ *************/ 

/*  THIS  TRAJECTORY  GENERATOR  WORKS  WITH  DEGREES,  NOT  RADIANS  */ 

/******** *****************************************************/ 
do_traj_z(&depth_traj,  rtime,&px,  &pxdot); 
des_depth  =  depth_traj.x; 

if  (depth_traj  .phase  ==  TR_ACCELERATING)  ACCEL  =1; 
if  (depth_traj  .phase  ==  TR_DECELERATING)  ACCEL  =-l; 

if  (depth_traj .phase  ==  TR_CONSTANT_VELOCITY  II  depth_traj .phase  ==TR_DONE) 
ACCEL  =0; 
if  (speed  <  0.5) 
speed  =  0.5; 


des_depth_rate  =  depthjxaj.xdot; 
depth_accel=  (depth_rate  -  old_depth_rate)/dt; 
heave_accel=  (heave_rate  -  old_heave_rate)/dt; 
des_depth_accel  =  ACCEL*depth_traj.amax*sgn(des_depth_rate); 
des_heave_accel  =  0.0; 

I*  *  *  *********************************  *  j 


z_tilde  =  depth  -  des_depth; 
z_tilde_dot  =  depth_rate  -  des_depth_rate; 
s_z  =  z_tilde_dot  +  lambda_z  *  z_tilde; 
s_delta_z=  (s_z-phi_z*sat(s_z/phi_z)); 


des_pitch_rate  =  -R2D(((des_depth_accel  +  heave_rate*cos(D2R(pitch))) 

/  (heave*  sin(D2R(pitch))+  speed*cos(D2R(pitch)))) 

-k2_p/2.5  * s_z)  ;/*  IN  DEGREES/SEC  */ 

des_pitch_rate  =  limit(  des_pitch_rate,maxhr,-maxhr); 

pitch_accel=  (pitch_rate  -  old_pitch_rate)/dt;/*  IN  DEGREES/SECA2  */ 

des_pitch_accel  =(des_pitch_rate  -  old_des_pitch_rate)/dt;  /*  IN  DEGREES/SECA2  */ 

des_pitch_accel  =  limit(  des_pitch_accel,amax_hdg,-amax_hdg); 

des_pitch_int  +=  des_pitch_rate*dt; 

des_pitch  =  limit(  des_pitch_int,  pitch_limit,  -pitchjimit); 
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/*  set  up  basic  parameters  */ 
x_tilde  =  depth-  des_depth; 

W_tilde  =  heave_rate  -  des_heave_rate; 

theta_tilde  =  pitch  -  des_pitch;  /*  IN  DEGREES  */ 

q_tilde  =  pitch_rate  -  des_pitch_rate;/*  IN  DEGREES/SEC  */ 
ref_heave_rate  =  des_heave_rate  -  lambda_p  *  x_tilde; 

ref_pitch_vel  =  des_pitch_rate  -  lambda_p  *  theta_tilde;/*  IN  DEGREES/SEC  */ 
ref_pitch_accel  =  des_pitch_accel  -  lambda_p*q_tilde;/*  IN  DEGREES/SECA2  */ 
ref_heave_accel  =  des_heave_accel  -  lambda_p*W_tilde; 
s2=  D2R(q_tilde  +  lambda Jheta  *  theta_tilde);/*  IN  RADIANS/SEC  */ 
s_delta_2=  (s2-phi*sat(s2/phi));/*  IN  RADIANS/SEC  */ 

if  (rtime  <=  2.*dt){ 

B22  =-(  sensor(  REAL_VEH_W  )*Xg  -  sensor(  REAL_VEH_B  )*Xb); 

} 

else  { 

B22=-  Ahat[10]; 

} 

/*  calculate  the  new  guesstimates  on  the  system  parameters  */ 
if  (fabs(des_speed-speed)<=0.2*des_speed  &&  rtime  >=  60)  { 

Ahatdot[5  ] =-G AMM A  [5 ]  *  s_delta_2 *heave_r ate  ; 

Ahatdot[6]=-GAMMA[6] *s_delta_2*ref_pitch_accel ; 
Ahatdot[7]=-GAMMA[7]*s_delta_2*speed*heave; 
Ahatdot[8]=-GAMMA[8]*s_delta_2*speed*  ref_pitch_vel; 
Ahatdot[9]=-GAMMA[9]*s_delta_2*sin(D2R(pitch)); 

Ahatdot[  1 0]  =-GAMM  A  [10]*  s_delta_2*cos(D2R(pitch)) ; 


Ahat[5]+=  Ahatdot[5]*dt; 
Ahat[6]+=  Ahatdot[6]*dt; 
Ahat[7]+=  Ahatdot[7]*dt; 
Ahat[8]+=  Ahatdot[8]*dt; 
Ahat[9]+=  Ahatdot[9]*dt; 
Ahat[10]+=  Ahatdot[10]*dt; 


Ahat[5]  =  limit(  Ahat[5],  Ahat_max[5],Ahat_min[5]); 
Ahat[6]  =  limit(  Ahat[6],  Ahat_max[6],Ahat_min[6]); 
Ahat[7]  =  limit(  Ahat[7],  Ahat_max[7],Ahat_min[7]); 
Ahat[8]  =  limit(  Ahat[8],  Ahat_max[8],Ahat_min[8]); 
Ahat[9]  =  limit(  Ahat[9],  Ahat_max[9],Ahat_min[9]); 
Ahat[10]  =  limit(  Ahat[10],  -B22*1.5,-B22*-1.5); 

} 
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/*  THIS  PROGRAM  WORKS  WITH  DEGREES,  NOT  RADIANS  .  VALUES  FOR 
ANGLES  WITH  A  FEW  EXCEPTIONS  ARE  STORED  IN  THE  DATA  STRUCTURE 
AS  RADIANS  BUT  ARE  CONVERTED  TO  DEGREES  FOR  MOST  CALCULA¬ 
TIONS.  FOR  THE  INPUT  CALCS,  TERMS  WITH  DEGREES  HAVE  TO  BE  CON¬ 
VERTED  BACK  TO  RADIANS  FOR  CONSISTENT  UNITS  */ 


M_elev_hat  =  speed* (Ahat[7]*heave  +  Ahat[8]*  D2R(ref_pitch_vel))  + 
Ahat[9]*sin(D2R(des_pitch))  +Ahat[5]*heave_rate  +  Ahat[6]*D2R(ref_pitch_accel)  + 
Ahat[  1 0]  *cos(D2R(des_pitch)) ; 


M_elev  =  M_elev_hat-k2_p*s2; 

/*  This  keeps  the  control  surfaces  from  going  crazy  during  speed  changes  */ 
if  (speed  <=  des_speed) 

control_surface_k=(des_speed*des_speed*  1025*3.3921  *  .0299); 
else 

control_surface_k=(speed*speed*1025*3.3921*  .0299); 

Del24=M_elev/(- 1 .069 1  *control_surface_k) ; 

Del24  =  hmit(  Del24,  MAX_CONTROL,  -MAX.CONTROL); 


/********************************************************************/ 


/*  THIS  PROGRAM  WORKS  WITH  DEGREES,  NOT  RADIANS  .  VALUES  FOR 
ANGLES  WITH  A  FEW  EXCEPTIONS  ARE  STORED  IN  THE  DATA  STRUCTURE 
AS  RADIANS  BUT  ARE  CONVERTED  TO  DEGREES  FOR  MOST  CALCULA¬ 
TIONS.  FOR  THE  INPUT  CALCS,  TERMS  WITH  DEGREES  HAVE  TO  BE  CON¬ 
VERTED  BACK  TO  RADIANS  FOR  CONSISTENT  UNITS  */ 


/*  If  the  change  in  actuator  position  isn’t  greater 
than  1/2  degree  it  isn’t  worth  it.  */ 

if  (fabs(sensor(C_ELEVATOR_ANGLE)-Del24)>=.5*(PI/180)*(l. 0/speed) ) 

{ 

update(  C_ELEVATOR_ANGLE,  Del24);  /*  positive  elevator  angle  >  dive  */ 

} 

else 

{ 

update(  C_ELEVATOR_ANGLE,  sensor(C_ELEVATOR_ANGLE)) ; 

} 

break; 
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A.3  Heading  Control 

/* - 

I 

lAdaptive  controller  for  Heading  control 
I 

IPhil  LeBas  10/96 


| _ */ 

if  (old_c_heading  !=  c_heading)  init_traj_hdg(heading,c_heading  ,amax_hdg, 
maxhr,rtime,&hdg_traj) ; 
do_traj_hdg(&hdg_traj,  rtime,&px,  &pxdot); 


y***************** ********************************************* ******/ 


/*  THIS  PROGRAM  WORKS  WITH  DEGREES,  NOT  RADIANS  .  VALUES  FOR 
ANGLES  WITH  A  FEW  EXCEPTIONS  ARE  STORED  IN  THE  DATA  STRUCTURE 
AS  RADIANS  BUT  ARE  CONVERTED  TO  DEGREES  FOR  MOST  CALCULA¬ 
TIONS.  FOR  THE  INPUT  CALCS,  TERMS  WITH  DEGREES  HAVE  TO  BE  CON¬ 
VERTED  BACK  TO  RADIANS  FOR  CONSISTENT  UNITS  */  , 


/*  added  calculation  for  the  adaptive  controller  */ 
des_head  =  hdg_traj.x;/*  IN  DEGREES  */ 

des_heading_rate  =  hdg_traj.xdot; 

if  (hdg_traj  .phase  ==  TR_ACCELERATING)  accel  =1; 
if  (hdg_traj .phase  ==  TR_DECELERATING)  accel  =-l; 

if  (hdg_traj .phase  ==  TR_CON STANT_VELOCIT Y  II  hdg_traj .phase  ==TR_DONE) 
accel  =0; 


/*  This  finds  the  shortest  direction  for  the  turn  */ 
deltaheading  =  hdg_traj.xend  -  hdg_traj.xstart; 
while(deltaheading  >  180)deltaheading  -=  360  ; 
while(deltaheading  <  -180)deltaheading  +=  360  ; 

des_head_accel  =  sgn(deltaheading)*accel*hdg_traj.amax; 

head_accel  =  (heading_rate  -  old_heading_rate)/dt; 

yaw_tilde  =  mod_180(heading  -  des_head); 

r_tilde  =  heading_rate  -  des_heading_rate; 

sl=  D2R(r_tilde  +  lambda_h  *  yaw_tilde); 

s_delta_l=  sl-PHI*sat(sl/PHI) ; 

des_sway  =  0; 

des_sway_rate  =0; 

v_tilde  =  sway  -  des_sway; 

ref_head_vel  =  des_heading_rate  -  lambda_h  *  yaw_tilde; 
ref_hdg_accel=  des_head_accel;/*  -  lambda_h  *  r_tilde;*/ 
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ref_sway  =  des_sway  ; 

ref_sway_rate  =  des_sway_rate  -  lambda_h  *  v_tilde; 

/*  calculate  the  new  guesstimates  on  the  system  parameters  */ 
Ahatdot[0]=-GAMMA[0]  *s_delta_l  *sway_rate; 

Ahatdot[l  ]=-GAMMA[  1]  *s_delta_l  *  ref_hdg_accel; 
Ahatdot[2]=-GAMMA[2]*s_delta_l  *  speed*sway; 

Ahatdot[3 ] =-G AMM A [3]  * s_delta_  1  *  speed*ref_head_vel; 

Ahat[0]+=  Ahatdot[0]*dt; 

Ahat[l]+=  Ahatdot[l]*dt; 

Ahat[2]+=  Ahatdot[2]*dt; 

Ahat[3]+=  Ahatdot[3]*dt; 

Ahat[4]+=  Ahatdot[4]*dt; 

Ahat[0]  =  limit(  Ahat[0],  Ahat_max[0],  Ahat_min[0]); 

Ahat[l]  =  limit(  Ahat[l],  Ahat_max[l],  Ahat_min[l]); 

Ahat[2]  =  limit(  Ahat[2],  Ahat_max[2],  Ahat_min[2]); 

Ahat[3]  =  limit(  Ahat[3],  Ahat_max[3],  Ahat_min[3]); 

Ahat[4]  =  hmit(  Ahat[4],  Ahat_max[4],  Ahat_min[4]); 

M_rudder_hat  =  speed*(Ahat[2]*sway  +  Ahat[3]*D2R(  ref_head_vel)) 
+Ahat[0]*sway_rate  +  Ahat[l]*D2R(ref_hdg_accel); 

M_rudder  =  M_rudder_hat  -  kl_h*sl; 

/*control_surface_k=(des_speed*des_speed*.5*1025*3.39*  .0299);*/ 

Del  1 3=M_rudder/(- 1 .069 1  *control_surface_k) ; 

Del  13  =  limit(  Dell3,  MAX_CONTROL,  -M AX_CONTROL) ; 

/*  If  the  change  in  actuator  position  isn’t  greater 
than  1/2  degree  it  isn’t  worth  it.  */ 

if  (fabs(sensor(C_RUDDER_ANGLE)-Del  1 3)  >=  .5* (PI/ 180)* (1.0/speed)  ) 

{ 

update(  C_RUDDER_ANGLE,  Dell3); 

} 

else 

{ 

update(  C_RUDDER_AN GLE,  sensor(C_RUDDER_ANGLE)) ; 

} 

/*Positive  rudder  means  turn  to  left/port*/ 

} 

/* - 

Store  some  vehicle  state  values  for  next  iteration.... 
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_ */ 

old_des_head  =  des_head; 
old_des_heading_rate  =  des_heading_rate; 
old_heading_rate  =  heading_rate; 
old_depth  =  depth; 
old_c_depth  =  c_depth; 
old_depth_rate  =  depth_rate; 
old_heave_rate  =  heave_rate; 

old_des_depth  =  des_depth; 
old_des_depth_rate  =  des_depth_rate; 
old_c_heading=  c_heading  ; 
old_head  =  heading; 
old_pitch  =  pitch; 
old_pitch_rate  =  pitch_rate; 
old_des_pitch  =  des_pitch; 
old_des_pitch_rate  =  des_pitch_rate; 
old_speed  =  speed; 
last_time  =  rtime; 

} 

A.4Heading  and  Pitch/Depth  Control  Functions 

/*  keeps  heading  between  0-2PI,  only  corrects  once  though  */ 
void  wrap_heading(double  *h) 

{ 

while(*h  >  TWOPI)*h  -=  TWOPI ; 
while(*h  <  0)*h  +=  TWOPI ; 

} 

/*==============:=:========================:=:======:===*/ 

/*  corrects  the  difference  between  2  headings  to  keep  the  value  between 
+-  pi.  In  other  words,  it  takes  the  shortest  distance  around  the  circle 
*/ 

double  delta_heading(double  dh) 

{ 

double  result ; 
result  =  dh ; 

while(result  >  PI)result  -=  TWOPI ; 
while(result  <  -PI)result  +=  TWOPI ; 
retum(result) ; 

} 

/*===zz=:zz======2— —====— ====——=====—*/ 

double  delta_heading_deg(double  dh) 
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double  result ; 
result  =  dh ; 

while(result  >  180)result  -=  360  ; 
while(result  <  -180)result  +=  360  ; 
retum(result) ; 

} 

double  dista(double  accel,  double  t) 

{ 

retum(.5*accel*t*t); 

} 

I  LIMIT: 

I  Makes  sure  a  variable  falls  within  defined  limits... 


double  limit(  double  value,  double  max,  double  min ) 

{ 

double  dummy; 

if  ( max  <  min ) 

{ 

dummy  =  max; 
max  =  min; 
min  =  dummy; 

} 

if  (value  >  max) 
value  =  max; 
if  (value  <  min) 
value  =  min; 
return  value; 

} 

/*============= 

static  double  sgn(  double  value ) 

{ 

double  sign; 

if  (value  <  0.0) 
sign  =  -1.0; 

if  (value  >=  0.0) 
sign  =  1.0; 
return  sign; 


} 

/* - ===========================*/ 

static  double  sat(double  x) 

{ 

double  y; 
if  ( fabs(x)  >  1. ) 
y  =  sgn(x); 

else 
y  =  x; 
return  y; 

} 

/* - =============:=========*/ 

static  double  mod_180(double  angle) 

{ 

if(fabs(angle)>360.0) 

angle  =  angle  -  (double)((int)(angle/360.0))*360.0; 
if(angle>=  180.0) 
angle  =  angle  -  360.0; 
if(angle<- 180.0) 
angle  =  angle  +  360.0; 
retum(angle); 

} 


A.5  Trajectory  Generator 

/*  initialize  the  trajectory  structure 

you  also  need  to  set  the  trajectory  displacement  (.x)  to  the 

current  hdg  state  to  initialize*/ 

void  init_traj_hdg(double  xstart,  double  xend,  double  amax,  double  vmax,  double  t, 
traj_new  *tr) 

{ 

double  taccel,  dx,  xtriangle,  xconst_vel ; 

tr->tstart  =  t ; 
tr->xstart  =  xstart ; 
tr->xend  =  xend ; 
tr->amax  =  fabs(amax) ; 
tr->vmax  =  fabs(vmax) ; 
tr->x  =  xstart ; 
tr->xdot  =  0.0; 

tr->phase  =  TR_ACCELERATING; 
taccel  =  tr->vmax/tr->amax; 
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/*  compute  the  distance  traveled  if  we  accelerate  to  max  speed  and 
then  decelerate  back  to  zero  speed  */ 
xtriangle  =  tr->amax*  taccel  *taccel ; 
dx  =  delta_heading_deg(xend-xstart) ; 

/*  test  if  the  needed  velocity  profile  is  a  trapezoid  */ 
if(fabs(dx)  >  xtriangle) 

{ 

tr->tl  =  tr->tstart  +  taccel ; 

xconst_vel  =  fabs(dx)-xtriangle  ;  /*  distance  covered  at  constant  vel*/ 
tr->t2  =  tr->tl  +  xconst_vel/tr->vmax  ; 
tr->tend  =  tr->t2  +  taccel ; 
tr->type  =  TR_TRAPEZOIDAL ; 

} 

else 

{ 

taccel  =  sqrt(fabs(dx)/tr->amax) ; 
tr->tl=tr->t2  =  tr->tstart  +  taccel ; 
tr->tend  =  tr->tl+taccel ; 
tr->type  =  TR_TRI ANGULAR  ; 

} 

} 

void  init_traj_z(double  xstart,  double  xend,  double  amax,  double  vmax,  double  t,  traj_new 
*tr) 

{ 

double  taccel,  dx,  xtriangle,  xconst_vel,deltat ; 
tr->tstart  =  t ; 

tr->xstart  =  xstart  ;/*depth  */ 
tr->xend  =  xend  ;/*  c_depth  */ 
tr->amax  =  fabs(amax) ; 
tr->vmax  =  fabs(vmax)/2  ; 
tr->x  =  xstart ; 
tr->xdot  =  0.0; 

tr->phase  =  TR_ACCELERATING; 
taccel  =  tr->vmax/tr->amax; 

/*  compute  the  distance  traveled  if  we  accelerate  to  max  speed  and 
then  decelerate  back  to  zero  speed  */ 
xtriangle  =  tr->amax*taccel*taccel ; 
dx  =  xend-xstart ; 

/* 

fprintf(stderr, "INITIALIZATION  time  =  %5.3f  xdot  =  %5.3f  amax  =  %5.3f,vmax  = 
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%5.3f\n "  ,t,tr->xdot,tr->amax,tr->vmax); 

*/ 

/*  test  if  the  needed  velocity  profile  is  a  trapezoid  */ 
if(fabs(dx)  >  xtriangle) 

{ 

tr->tl  =  tr->tstart  +  taccel ; 

xconst_vel  =  fabs(dx)-xtriangle  ;  /*  distance  covered  at  constant  vel*/ 
tr->t2  =  tr->tl  +  xconst_vel/tr->vmax  ; 
tr->tend  =  tr->t2  +  taccel ; 
tr->type  =  TR_TRAPEZOIDAL ; 

} 

else 


{ 

taccel  =  sqrt(fabs(dx)/tr->amax) ; 
tr->tl=tr->t2  =  tr->tstart  +  taccel ; 
tr->tend  =  tr->tl+taccel ; 


tr->type  =  TR_TRIANGULAR ; 

} 


} 

/*********************************************************************/ 

void  do_traj_hdg(traj_new  *tr,  double  t,  double  *px,  double  *pxdot) 

{ 


double  dx,  s,  dt ; 

dx  =  delta_heading_deg(tr->xend-tr->xstart) ; 
dt=.02; 

/*  s  contains  the  sign  of  the  position  change  from  xstart  to  xend  */ 
s  =  1.0 ; 

if(dx  <  0.0)s=-1.0 ; 
if(t<  tr->tl) 

{ 

dt  =  t  -  tr->tstart ; 

*pxdot  =  s*tr->amax*dt ; 

*px  =  tr->xstart  +  s*dista(tr->amax,dt) ; 
tr->phase  =  TR.ACCELERATING ; 

} 

else 

{ 

if(  (t  <  tr->t2)  &&  (tr->type  ==  TR_TRAPEZOIDAL)) 

{ 

dt  =  t-tr->tl  ; 

*pxdot  =  s*tr->vmax ; 
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*px  =  tr->xstart  +  s  *dista(tr->amax,  (tr->t  1  -tr->tstart))  + 

*pxdot*dt ; 

tr->phase  =  TR_CONSTANT_VELOCITY; 

} 

else 

{ 

if(t  <  tr->tend) 

{ 

dt  =  tr->tend  - 1; 

*pxdot  =  s*tr->amax*dt ; 

*px  =  tr->xend  -  s*dista(tr->amax,dt) ; 
tr->phase  =  TR.DECELERATING; 

} 

else 

{ 

*px  =  ALPHA_TR*tr->x  +  BETA_TR*tr->xend  ; 

*pxdot  =  0.0 ; 
tr->phase  =  TR_DONE; 

} 

} 

} 

tr->x  =  *px  ; 
tr->xdot  =  *pxdot ; 

} 

/*============:=:======:=======:===========;==:======;=====*/ 

void  do_traj_z(traj_new  *tr,  double  t,  double  *px,  double  *pxdot) 

{ 


double  dx,  s,  dt ; 

dx  =  tr->xend-tr->xstart ; 

dt=.02; 

/*  s  contains  the  sign  of  the  position  change  from  xstart  to  xend  */ 
s  =  1.0 ; 

if(dx  <  0.0)s=-1.0 ; 
if(t  <  tr->tl) 

{ 

dt  =  t  -  tr->tstart ; 

*pxdot  =  s*tr->amax*dt ; 

*px  =  tr->xstart  +  s*dista(tr->amax,dt) ; 
tr->phase  =  TR_ACCELERATING  ; 

} 

else 

{ 
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if(  (t  <  tr->t2)  &&  (tr->type  ==  TR_TRAPEZOIDAL)) 

{ 

dt  =  t-tr->tl  ; 

*pxdot  =  s*tr->vmax  ; 

*px  =  tr->xstart  +  s*dista(tx->amax,(tr->tl-tr->tstart))  + 

*pxdot*dt ; 

tr->phase  =  TR_CON  STANT_VELOCIT  Y ; 

} 

else 

{ 

if(t  <  tr->tend) 

{ 

dt  =  tr->tend  - 1; 

*pxdot  =  s*tr->amax*dt ; 

*px  =  tr->xend  -  s*dista(tr->amax,dt) ; 
tr->phase  =  TR_DECELERATING; 

} 

else 

{ 

*px  =  ALPHA_TR*tr->x  +  BETA_TR*tr->xend  ; 
*pxdot  =  0.0 ; 
tr->phase  =  TR_DONE; 

} 

} 

} 

tr->x  =  *px  ; 
tr->xdot  =  *pxdot ; 


} 
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